btl143/QD4C-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: 增加分段pid

Browse Source
This commit is contained in:
井明江
2024-03-28 04:24:59 +08:00
parent 99c582c53e
commit 8020561175
11 changed files with 278 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
app/by_fan_control.c
View File

@@ -1,6 +1,7 @@
#include <stdio.h>
#include "by_fan_control.h"
#include "zf_common_headfile.h"
#include "jj_blueteeth.h"
#define FAN_LL_PWM_PIN TIM8_PWM_MAP0_CH1_C6 // 左升力风扇
#define FAN_RL_PWM_PIN TIM8_PWM_MAP0_CH2_C7 // 右升力风扇
@@ -74,14 +75,13 @@ void by_pwm_power_duty(uint32_t pwm_duty_ls, uint32_t pwm_duty_rs, uint32_t pwm_
{
pwm_duty_ls = myclip(pwm_duty_ls, 0, 8000);
pwm_duty_rs = myclip(pwm_duty_rs, 0, 8000);
pwm_duty_lb = myclip(pwm_duty_lb, 0, 6000);
pwm_duty_rb = myclip(pwm_duty_rb, 0, 6000);
pwm_duty_lb = myclip(pwm_duty_lb, 0, 8000);
pwm_duty_rb = myclip(pwm_duty_rb, 0, 8000);
pwm_duty_ls_g = pwm_duty_ls;
pwm_duty_rs_g = pwm_duty_rs;
pwm_duty_lb_g = pwm_duty_lb;
pwm_duty_rb_g = pwm_duty_rb;
pwm_set_duty(FAN_LS_PWM_PIN, pwm_duty_ls);
pwm_set_duty(FAN_RS_PWM_PIN, pwm_duty_rs);
pwm_set_duty(FAN_LB_PWM_PIN, pwm_duty_lb);

68
app/jj_motion.c
View File

@@ -12,23 +12,35 @@ PID_TypeDef far_gyro_pid;
PID_TypeDef near_pos_pid;
PID_TypeDef speed_pid;
float an_Kp = 8.0f;
float an_Ki = 1.0f;
float an_Kd = 2.0f;
float an_Kp0 = 8.0f;
float an_Ki0 = 1.0f;
float an_Kd0 = 2.0f;
float an_Kp1 = 8.0f;
float an_Ki1 = 1.0f;
float an_Kd1 = 2.0f;
float in_angle;
float set_angle = 0.0f;
float out_angle;
float gy_Kp = 1.0f;
float gy_Ki = 0.0f;
float gy_Kd = 0.0f;
float gy_Kp0 = 1.0f;
float gy_Ki0 = 0.0f;
float gy_Kd0 = 0.0f;
float gy_Kp1 = 1.0f;
float gy_Ki1 = 0.0f;
float gy_Kd1 = 0.0f;
float in_gyro;
float out_gyro;
// float set_gyro = 0.0f;
float po_Kp = 1.0f;
float po_Ki = 0.0f;
float po_Kd = 0.0f;
float po_Kp0 = 1.0f;
float po_Ki0 = 0.0f;
float po_Kd0 = 0.0f;
float po_Kp1 = 1.0f;
float po_Ki1 = 0.0f;
float po_Kd1 = 0.0f;
float in_pos;
float out_pos;
float set_pos = 0.0f;
@@ -38,10 +50,16 @@ float sp_Ki = 0.0f;
float sp_Kd = 0.0f;
float in_speed;
float out_speed;
float set_speed = 0.0f;
float set_speed0 = 0.0f;
float set_speed1 = 0.0f;
int cnt1 = 0;
int cnt2 = 0;
uint32_t in_state = 0;
uint32_t pwm_duty_ls;
uint32_t pwm_duty_rs;
uint32_t pwm_duty_lb;
uint32_t pwm_duty_rb;
static float myclip_f(float x, float low, float up)
{
@@ -67,6 +85,11 @@ float sport_get_speed(void)
void sport_motion(void)
{
if (1 == in_state) {
PID_SetTunings(&far_gyro_pid, an_Kp1, an_Ki1, an_Kd1);
} else {
PID_SetTunings(&far_gyro_pid, an_Kp0, an_Ki0, an_Kd0);
}
/* 动力风扇设置 */
if (1 == bt_run_flag) {
cnt1++;
@@ -74,9 +97,6 @@ void sport_motion(void)
imu660ra_get_gyro();
in_gyro = imu660ra_gyro_z; // 陀螺仪输入
// in_angle = 0; // 图像远端输入
// in_pos = 0; // 图像近端输入
// 清除计数
PID_Compute(&far_gyro_pid);
if (cnt1 >= 10) {
@@ -87,18 +107,16 @@ void sport_motion(void)
cnt1 = 0;
}
if (cnt2 >= 20) {
cnt2 = 0;
PID_Compute(&far_angle_pid);
}
uint32_t pwm_duty_ls = (uint32_t)myclip_f(-1 * out_pos + out_gyro, 0.0f, 8000.f);
uint32_t pwm_duty_rs = (uint32_t)myclip_f(1 * out_pos - out_gyro, 0.0f, 8000.f);
uint32_t pwm_duty_lb = (uint32_t)myclip_f(1 * out_speed + out_gyro, 0.0f, 6000.f);
uint32_t pwm_duty_rb = (uint32_t)myclip_f(1 * out_speed - out_gyro, 0.0f, 6000.f);
by_pwm_power_duty(pwm_duty_ls, pwm_duty_rs, pwm_duty_lb, pwm_duty_rb);
pwm_duty_ls = (uint32_t)myclip_f(-1.5f * out_pos + out_gyro, 0.0f, 8000.f);
pwm_duty_rs = (uint32_t)myclip_f(1.5f * out_pos - out_gyro, 0.0f, 8000.f);
pwm_duty_lb = (uint32_t)myclip_f(1.f * out_speed + out_gyro, 0.0f, 8000.f);
pwm_duty_rb = (uint32_t)myclip_f(1.f * out_speed - out_gyro, 0.0f, 8000.f);
// by_pwm_power_duty(500+out_gyro, 500-out_gyro, 500+out_gyro , 500-out_gyro);
by_pwm_power_duty(pwm_duty_ls, pwm_duty_rs, pwm_duty_lb, pwm_duty_rb);
} else {
by_pwm_power_duty(0, 0, 0, 0);
}
@@ -118,28 +136,28 @@ void sport_motion(void)
void sport_pid_init()
{
/* 角度控制 */
PID(&far_angle_pid, &in_angle, &out_angle, &set_angle, an_Kp, an_Ki, an_Kd, _PID_P_ON_E, _PID_CD_REVERSE);
PID(&far_angle_pid, &in_angle, &out_angle, &set_angle, an_Kp1, an_Ki1, an_Kd1, _PID_P_ON_E, _PID_CD_REVERSE);
PID_SetMode(&far_angle_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&far_angle_pid, 20);
PID_SetOutputLimits(&far_angle_pid, -3000.0f, 3000.0f);
// PID_Init(&far_angle_pid);
/* 角速度控制 */
PID(&far_gyro_pid, &in_gyro, &out_gyro, &out_angle, gy_Kp, gy_Ki, gy_Kd, _PID_P_ON_E, _PID_CD_REVERSE);
PID(&far_gyro_pid, &in_gyro, &out_gyro, &out_angle, gy_Kp1, gy_Ki1, gy_Kd1, _PID_P_ON_E, _PID_CD_REVERSE);
PID_SetMode(&far_gyro_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&far_gyro_pid, 1);
PID_SetOutputLimits(&far_gyro_pid, -4000.0f, 4000.0f);
// PID_Init(&far_gyro_pid);
/* 近点控制 */
PID(&near_pos_pid, &in_pos, &out_pos, &set_pos, po_Kp, po_Ki, po_Kd, _PID_P_ON_E, _PID_CD_DIRECT);
PID(&near_pos_pid, &in_pos, &out_pos, &set_pos, po_Kp1, po_Ki1, po_Kd1, _PID_P_ON_E, _PID_CD_DIRECT);
PID_SetMode(&near_pos_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&near_pos_pid, 10);
PID_SetOutputLimits(&near_pos_pid, -4000.0f, 4000.0f);
// PID_Init(&near_pos_pid);
/* 速度控制 */
PID(&speed_pid, &in_speed, &out_speed, &set_speed, sp_Kp, sp_Ki, sp_Kd, _PID_P_ON_E, _PID_CD_DIRECT);
PID(&speed_pid, &in_speed, &out_speed, &set_speed1, sp_Kp, sp_Ki, sp_Kd, _PID_P_ON_E, _PID_CD_DIRECT);
PID_SetMode(&speed_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&speed_pid, 10);
PID_SetOutputLimits(&speed_pid, 0.0f, 2000.0f);

35
app/jj_motion.h
View File

@@ -3,23 +3,32 @@
#include "ch32v30x.h"
#include "../3rd-lib/PID-Library/pid.h"
extern float an_Kp;
extern float an_Ki;
extern float an_Kd;
extern float an_Kp0;
extern float an_Ki0;
extern float an_Kd0;
extern float an_Kp1;
extern float an_Ki1;
extern float an_Kd1;
extern float in_angle;
extern float set_angle;
extern float out_angle;
extern float gy_Kp;
extern float gy_Ki;
extern float gy_Kd;
extern float gy_Kp1;
extern float gy_Ki1;
extern float gy_Kd1;
extern float gy_Kp0;
extern float gy_Ki0;
extern float gy_Kd0;
extern float in_gyro;
extern float out_gyro;
extern float set_gyro;
extern float po_Kp;
extern float po_Ki;
extern float po_Kd;
extern float po_Kp1;
extern float po_Ki1;
extern float po_Kd1;
extern float po_Kp0;
extern float po_Ki0;
extern float po_Kd0;
extern float in_pos;
extern float out_pos;
extern float set_pos;
@@ -29,8 +38,14 @@ extern float sp_Ki;
extern float sp_Kd;
extern float in_speed;
extern float out_speed;
extern float set_speed;
extern float set_speed0;
extern float set_speed1;
extern uint32_t pwm_duty_ls;
extern uint32_t pwm_duty_rs;
extern uint32_t pwm_duty_lb;
extern uint32_t pwm_duty_rb;
extern uint32_t in_state;
void sport_pid_init();
void sport_motion(void);
#endif

34
app/jj_param.c
View File

@@ -17,23 +17,37 @@ void jj_param_eeprom_init(void)
{
soft_iic_init(&eeprom_param, K24C02_DEV_ADDR, K24C02_SOFT_IIC_DELAY, K24C02_SCL_PIN, K24C02_SDA_PIN); // eeprom初始化
PARAM_REG(angle_Kp, &an_Kp, EFLOAT, 1, "an_P:"); // 注冊
PARAM_REG(angle_Ki, &an_Ki, EFLOAT, 1, "an_I:"); // 注冊
PARAM_REG(angle_Kd, &an_Kd, EFLOAT, 1, "an_D:");
PARAM_REG(angle_Kp0, &an_Kp0, EFLOAT, 1, "an_P0:"); // 注冊
PARAM_REG(angle_Ki0, &an_Ki0, EFLOAT, 1, "an_I0:"); // 注冊
PARAM_REG(angle_Kd0, &an_Kd0, EFLOAT, 1, "an_D0:");
PARAM_REG(gyro_Kp, &gy_Kp, EFLOAT, 1, "gy_P:"); // 注冊
PARAM_REG(gyro_Ki, &gy_Ki, EFLOAT, 1, "gy_I:"); // 注冊
PARAM_REG(gyro_Kd, &gy_Kd, EFLOAT, 1, "gy_D:");
PARAM_REG(gyro_Kp0, &gy_Kp0, EFLOAT, 1, "gy_P0:"); // 注冊
PARAM_REG(gyro_Ki0, &gy_Ki0, EFLOAT, 1, "gy_I0:"); // 注冊
PARAM_REG(gyro_Kd0, &gy_Kd0, EFLOAT, 1, "gy_D0:");
PARAM_REG(speed_Kp, &sp_Kp, EFLOAT, 1, "sp_P:"); // 注冊
PARAM_REG(speed_Ki, &sp_Ki, EFLOAT, 1, "sp_I:"); // 注冊
PARAM_REG(speed_Kd, &sp_Kd, EFLOAT, 1, "sp_D:");
PARAM_REG(pos_Kp, &po_Kp, EFLOAT, 1, "po_P:"); // 注冊
PARAM_REG(pos_Ki, &po_Ki, EFLOAT, 1, "po_I:"); // 注冊
PARAM_REG(pos_Kd, &po_Kd, EFLOAT, 1, "po_D:");
PARAM_REG(pos_Kp0, &po_Kp0, EFLOAT, 1, "po_P0:"); // 注冊
PARAM_REG(pos_Ki0, &po_Ki0, EFLOAT, 1, "po_I0:"); // 注冊
PARAM_REG(pos_Kd0, &po_Kd0, EFLOAT, 1, "po_D0:");
PARAM_REG(param_set_speed, &set_speed, EFLOAT, 1, "rate:");
PARAM_REG(param_set_speed0, &set_speed0, EFLOAT, 1, "rate0:");
PARAM_REG(angle_Kp1, &an_Kp1, EFLOAT, 1, "an_P1:"); // 注冊
PARAM_REG(angle_Ki1, &an_Ki1, EFLOAT, 1, "an_I1:"); // 注冊
PARAM_REG(angle_Kd1, &an_Kd1, EFLOAT, 1, "an_D1:");
PARAM_REG(gyro_Kp1, &gy_Kp1, EFLOAT, 1, "gy_P1:"); // 注冊
PARAM_REG(gyro_Ki1, &gy_Ki1, EFLOAT, 1, "gy_I1:"); // 注冊
PARAM_REG(gyro_Kd1, &gy_Kd1, EFLOAT, 1, "gy_D1:");
PARAM_REG(pos_Kp1, &po_Kp1, EFLOAT, 1, "po_P1:"); // 注冊
PARAM_REG(pos_Ki1, &po_Ki1, EFLOAT, 1, "po_I1:"); // 注冊
PARAM_REG(pos_Kd1, &po_Kd1, EFLOAT, 1, "po_D1:");
PARAM_REG(param_set_speed1, &set_speed1, EFLOAT, 1, "rate1:");
jj_param_read(); // 注冊
}
/**

30
app/jj_param.h
View File

@@ -18,21 +18,31 @@ typedef enum {
Page1_head = 0,
angle_Kp = Page1_head,
angle_Ki,
angle_Kd,
gyro_Kp,
gyro_Ki,
gyro_Kd,
angle_Kp0 = Page1_head,
angle_Ki0,
angle_Kd0,
gyro_Kp0,
gyro_Ki0,
gyro_Kd0,
speed_Kp,
speed_Ki,
speed_Kd,
pos_Kp,
pos_Ki,
pos_Kd,
param_set_speed,
pos_Kp0,
pos_Ki0,
pos_Kd0,
param_set_speed0,
Page2_head,
angle_Kp1 = Page2_head,
angle_Ki1,
angle_Kd1,
gyro_Kp1,
gyro_Ki1,
gyro_Kd1,
pos_Kp1,
pos_Ki1,
pos_Kd1,
param_set_speed1,
DATA_IN_FLASH_NUM,
delta_x,

3
app/main.c
View File

@@ -59,7 +59,7 @@ int main(void)
pit_ms_init(TIM1_PIT, 1); // 运动解算，编码器
printf("ok\r\n");
bt_printf("ok\r\n");
while (1) {
// printf("pwm:%lu,%lu,%lu,%lu\r\n", pwm_duty_ls_g, pwm_duty_rs_g, pwm_duty_lb_g, pwm_duty_rb_g);
@@ -69,5 +69,6 @@ int main(void)
jj_bt_run();
in_pos = test_data[1].f32;
in_angle = test_data[0].f32;
in_state= test_data[2].u32;
}
}

3
app/page/page.c
View File

@@ -125,7 +125,8 @@ void Page_Init(void)
{
PAGE_REG(page_menu);
// PAGE_REG(page_rtcam);
PAGE_REG(page_param);
PAGE_REG(page_param_pid0);
PAGE_REG(page_param_pid1);
// PAGE_REG(page_argv);
// PAGE_REG(page_sys);
// PAGE_REG(page_run);

3
app/page/page.h
View File

@@ -20,7 +20,8 @@ enum PageID {
//......
page_menu,
//page_rtcam,
page_param,
page_param_pid0,
page_param_pid1,
page_dparam,
// page_argv,
// page_sys,

9
app/page/page_dparam.c
View File

@@ -56,6 +56,15 @@ static void Loop()
ips200_show_float(80, 140, out_gyro, 4, 1);
ips200_show_string(0, 160, "outpos");
ips200_show_float(80, 160, out_pos, 4, 1);
ips200_show_string(0, 180, "ls");
ips200_show_float(80, 180, pwm_duty_ls, 4, 1);
ips200_show_string(0, 200, "rs");
ips200_show_float(80, 200, pwm_duty_rs, 4, 1);
ips200_show_string(0, 220, "lb");
ips200_show_float(80, 220, pwm_duty_lb, 4, 1);
ips200_show_string(100, 0, "rb");
ips200_show_float(180, 0, pwm_duty_rb, 4, 1);
}
/**
* @brief 页面事件

16
app/page/page_param.c → app/page/page_param_pid0.c
View File

@@ -5,13 +5,13 @@
#include <math.h>
#define LINE_HEAD 0
#define LINE_END DATA_NUM - 2
static char Text[] = "RealTime Param";
int event_flag = 0;
int32_t index_power = 0;
#define LINE_END Page2_head - 1
static char Text[] = "Param_pid0";
static int event_flag = 0;
static int index_power = 0;
static int8_t Curser = LINE_HEAD; // 定义光标位置
static int8_t Curser_Last = LINE_HEAD; // 定义光标位置
void jj_param_show();
static void jj_param_show();
/***************************************************************************************
*
* 以下为页面模板函数
@@ -26,7 +26,7 @@ static void Setup()
{
ips200_clear();
Print_Curser(Curser, Curser_Last, RGB565_PURPLE);
for (int16 i = 0; i < DATA_IN_FLASH_NUM - 1; i++) {
for (int16 i = LINE_HEAD; i < LINE_END; i++) {
ips200_show_string(0, i * 18 + 2, Param_Data[i].text);
if (Param_Data[i].type == EINT32)
ips200_show_int(50, i * 18 + 2, *((int32 *)(Param_Data[i].p_data)), 5);
@@ -116,7 +116,7 @@ static void Event(page_event event)
jj_param_show();
}
}
void jj_param_show()
static void jj_param_show()
{
if (EINT32 == Param_Data[Curser].type)
ips200_show_int(50, Curser * 18 + 2, *((int32 *)(Param_Data[Curser].p_data)), 5);
@@ -130,7 +130,7 @@ void jj_param_show()
*
* @param pageID
*/
void PageRegister_page_param(unsigned char pageID)
void PageRegister_page_param_pid0(unsigned char pageID)
{
Page_Register(pageID, Text, Setup, Loop, Exit, Event);
}

136
app/page/page_param_pid1.c Normal file
View File

@@ -0,0 +1,136 @@
#include "jj_param.h"
#include "page_ui_widget.h"
#include "jj_motion.h"
#include "page.h"
#include <math.h>
#define LINE_HEAD Page2_head
#define LINE_END DATA_IN_FLASH_NUM
static char Text[] = "Param_pid1";
static int event_flag = 0;
static int index_power = 0;
static int8_t Curser = LINE_HEAD; // 定义光标位置
static int8_t Curser_Last = LINE_HEAD; // 定义光标位置
static void jj_param_show();
/***************************************************************************************
*
* 以下为页面模板函数
*
***************************************************************************************/
/**
* @brief 页面初始化事件
* @param 无
* @retval 无
*/
static void Setup()
{
ips200_clear();
Print_Curser(Curser, Curser_Last, RGB565_PURPLE);
for (int16 i = 0; i <= DATA_IN_FLASH_NUM - Page2_head; i++) {
ips200_show_string(0, i * 18 + 2, Param_Data[i+Page2_head].text);
if (Param_Data[i].type == EINT32)
ips200_show_int(50, i * 18 + 2, *((int32 *)(Param_Data[i+Page2_head].p_data)), 5);
else if (Param_Data[i].type == EFLOAT)
ips200_show_float(50, i * 18 + 2, *((float *)(Param_Data[i+Page2_head].p_data)), 4, 5);
}
ips200_show_int(100, 2, index_power, 5);
}
/**
* @brief 页面退出事件
* @param 无
* @retval 无
*/
static void Exit()
{
}
/**
* @brief 页面循环执行的内容
* @param 无
* @retval 无
*/
static void Loop()
{
}
/**
* @brief 页面事件
* @param btn:发出事件的按键
* @param event:事件编号
* @retval 无
*/
static void Event(page_event event)
{
if (0 == event_flag) {
Curser_Last = Curser;
if (page_event_forward == event) {
Curser--; // 光标上移
} else if (page_event_backward == event) {
Curser++; // 光标下移
} else if (page_event_press_short == event) {
event_flag = 1; // 选中参数
Print_Curser(Curser, Curser_Last, RGB565_RED);
return;
} else if (page_event_press_long == event) {
jj_param_write();
sport_pid_init();
Page_Shift(page_menu);
return;
}
if (Curser < LINE_HEAD) {
Curser = LINE_END;
} else if (Curser > LINE_END) {
Curser = LINE_HEAD;
}
Print_Curser(Curser, Curser_Last, RGB565_PURPLE);
} else if (1 == event_flag) {
if (page_event_forward == event) {
if (Param_Data[Curser].type == EFLOAT) {
*((float *)(Param_Data[Curser].p_data)) += powf(10.0f,(float)index_power);
} else if (Param_Data[Curser].type == EINT32) {
*((int32 *)(Param_Data[Curser].p_data)) += 1;
} else if (Param_Data[Curser].type == EUINT32) {
*((uint32 *)(Param_Data[Curser].p_data)) += 1;
}
} else if (page_event_backward == event) {
if (Param_Data[Curser].type == EFLOAT) {
*((float *)(Param_Data[Curser].p_data)) -=powf(10.0f,(float)index_power);
} else if (Param_Data[Curser].type == EINT32) {
*((int32 *)(Param_Data[Curser].p_data)) -= 1;
} else if (Param_Data[Curser].type == EUINT32) {
*((uint32 *)(Param_Data[Curser].p_data)) -= 1;
}
} else if (page_event_press_short == event) {
index_power++;
if (index_power > 2) {
index_power = -2;
}
ips200_show_int(100, 2, index_power, 5);
} else if (page_event_press_long == event) {
event_flag = 0;
Print_Curser(Curser, Curser_Last, RGB565_PURPLE);
}
jj_param_show();
}
}
static void jj_param_show()
{
if (EINT32 == Param_Data[Curser+Page2_head].type)
ips200_show_int(50, Curser * 18 + 2, *((int32 *)(Param_Data[Curser+Page2_head].p_data)), 5);
else if (EUINT32 == Param_Data[Curser+Page2_head].type)
ips200_show_uint(50, Curser * 18 + 2, *((int32 *)(Param_Data[Curser+Page2_head].p_data)), 5);
else if (EFLOAT == Param_Data[Curser+Page2_head].type)
ips200_show_float(50, Curser * 18 + 2, *((float *)(Param_Data[Curser+Page2_head].p_data)), 4, 5);
}
/**
* @brief 页面注册函数
*
* @param pageID
*/
void PageRegister_page_param_pid1(unsigned char pageID)
{
Page_Register(pageID, Text, Setup, Loop, Exit, Event);
}