From ad2ba36b54340bbf8a340c7cde0cdfbd639ecee7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E4=BA=95=E6=98=8E=E6=B1=9F?= <246462502@qq.com> Date: Tue, 18 Jun 2024 16:11:59 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B2=E7=8E=87pid=E8=BE=93=E5=87=BA?= =?UTF-8?q?=E5=AD=98=E5=9C=A8=E8=B6=8A=E7=95=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .eide/eide.json | 3 +- app/by_fan_control.c | 79 ++++++++++++++---------------------------- app/jj_motion.c | 58 ++++++++++++++++--------------- app/jj_motion.h | 1 + app/main.c | 8 +++-- app/page/page_dparam.c | 22 ++++++------ 6 files changed, 77 insertions(+), 94 deletions(-) diff --git a/.eide/eide.json b/.eide/eide.json index 506dacb..8a70f99 100644 --- a/.eide/eide.json +++ b/.eide/eide.json @@ -68,10 +68,9 @@ "libList": [ "libraries/zf_device" ], - "sourceDirList": [], "defineList": [] } } }, - "version": "3.3" + "version": "3.4" } \ No newline at end of file diff --git a/app/by_fan_control.c b/app/by_fan_control.c index 5968a9a..2ba11c5 100644 --- a/app/by_fan_control.c +++ b/app/by_fan_control.c @@ -6,17 +6,17 @@ #define FAN_LL_PWM_PIN TIM8_PWM_MAP1_CH1_B6 // 左升力风扇 #define FAN_RL_PWM_PIN TIM8_PWM_MAP1_CH2_B7 // 右升力风扇 // M4 -#define FAN_LB_PWM_A_PIN TIM10_PWM_MAP3_CH2_D3 // 左侧动力风扇 -#define FAN_LB_PWM_B_PIN TIM10_PWM_MAP3_CH4_D7 // 左侧动力风扇 +#define FAN_LB_PWM_A_PIN TIM10_PWM_MAP3_CH2_D3 // 左后 +#define FAN_LB_PWM_B_PIN TIM10_PWM_MAP3_CH4_D7 // 左后 // M1 -#define FAN_RB_PWM_A_PIN TIM4_PWM_MAP1_CH1_D12 // 右侧动力风扇 -#define FAN_RB_PWM_B_PIN TIM4_PWM_MAP1_CH3_D14 // 右侧动力风扇 +#define FAN_RB_PWM_A_PIN TIM4_PWM_MAP1_CH1_D12 // 右后 +#define FAN_RB_PWM_B_PIN TIM4_PWM_MAP1_CH3_D14 // 右后 // M3 -#define FAN_LS_PWM_A_PIN TIM10_PWM_MAP3_CH1_D1 // 左后动力风扇 -#define FAN_LS_PWM_B_PIN TIM10_PWM_MAP3_CH3_D5 // 左后动力风扇 +#define FAN_LS_PWM_A_PIN TIM10_PWM_MAP3_CH3_D5 // 左侧 +#define FAN_LS_PWM_B_PIN TIM10_PWM_MAP3_CH1_D1 // 左侧 // M2 -#define FAN_RS_PWM_A_PIN TIM4_PWM_MAP1_CH4_D15 // 右后动力风扇 -#define FAN_RS_PWM_B_PIN TIM4_PWM_MAP1_CH2_D13 // 右后动力风扇 +#define FAN_RS_PWM_A_PIN TIM4_PWM_MAP1_CH4_D15 // 右侧 +#define FAN_RS_PWM_B_PIN TIM4_PWM_MAP1_CH2_D13 // 右侧 int32_t pwm_duty_ls_g; int32_t pwm_duty_rs_g; @@ -41,40 +41,24 @@ inline static float clip_f32(float x, float low, float up) : x); } +#define FIX_DRIVE 0 + void by_pwm_init(void) { pwm_init(FAN_LL_PWM_PIN, 50, 500); pwm_init(FAN_RL_PWM_PIN, 50, 500); - // 测试鸣叫 - // pwm_init(FAN_LS_PWM_A_PIN, 1500, 1000); - // pwm_init(FAN_LS_PWM_B_PIN, 4000, 0); - // pwm_init(FAN_RS_PWM_A_PIN, 1500, 1000); - // pwm_init(FAN_RS_PWM_B_PIN, 4000, 0); - // pwm_init(FAN_LB_PWM_A_PIN, 1500, 1000); - // pwm_init(FAN_LB_PWM_B_PIN, 4000, 0); - // pwm_init(FAN_RB_PWM_A_PIN, 1500, 1000); - // pwm_init(FAN_RB_PWM_B_PIN, 4000, 0); - // system_delay_ms(100); - // pwm_init(FAN_LS_PWM_A_PIN, 2000, 1000); - // pwm_init(FAN_LS_PWM_B_PIN, 4000, 0); - // pwm_init(FAN_RS_PWM_A_PIN, 2000, 1000); - // pwm_init(FAN_RS_PWM_B_PIN, 4000, 0); - // pwm_init(FAN_LB_PWM_A_PIN, 2000, 1000); - // pwm_init(FAN_LB_PWM_B_PIN, 4000, 0); - // pwm_init(FAN_RB_PWM_A_PIN, 2000, 1000); - // pwm_init(FAN_RB_PWM_B_PIN, 4000, 0); - // system_delay_ms(100); - // pwm_init(FAN_LS_PWM_A_PIN, 2500, 1000); - // pwm_init(FAN_LS_PWM_B_PIN, 4000, 0); - // pwm_init(FAN_RS_PWM_A_PIN, 2500, 1000); - // pwm_init(FAN_RS_PWM_B_PIN, 4000, 0); - // pwm_init(FAN_LB_PWM_A_PIN, 2500, 1000); - // pwm_init(FAN_LB_PWM_B_PIN, 4000, 0); - // pwm_init(FAN_RB_PWM_A_PIN, 2500, 1000); - // pwm_init(FAN_RB_PWM_B_PIN, 4000, 0); - // system_delay_ms(100); - +#if FIX_DRIVE==1 + pwm_init(FAN_LS_PWM_A_PIN, 12000, 1000); + pwm_init(FAN_LS_PWM_B_PIN, 12000, 1000); + pwm_init(FAN_RS_PWM_A_PIN, 12000, 1000); + pwm_init(FAN_RS_PWM_B_PIN, 12000, 1000); + pwm_init(FAN_LB_PWM_A_PIN, 12000, 1000); + pwm_init(FAN_LB_PWM_B_PIN, 12000, 1000); + pwm_init(FAN_RB_PWM_A_PIN, 12000, 1000); + pwm_init(FAN_RB_PWM_B_PIN, 12000, 1000); + while (1); +#endif pwm_init(FAN_LS_PWM_A_PIN, 12000, 0); pwm_init(FAN_LS_PWM_B_PIN, 12000, 0); pwm_init(FAN_RS_PWM_A_PIN, 12000, 0); @@ -83,18 +67,7 @@ void by_pwm_init(void) pwm_init(FAN_LB_PWM_B_PIN, 12000, 0); pwm_init(FAN_RB_PWM_A_PIN, 12000, 0); pwm_init(FAN_RB_PWM_B_PIN, 12000, 0); - // while (1); - // system_delay_ms(3000); - // // pwm_init(FAN_LS_PWM_PIN, 50, 1000); // 动力风扇左 1 - // // pwm_init(FAN_RS_PWM_PIN, 50, 1000); // 动力风扇右 1 - // // pwm_init(FAN_LB_PWM_PIN, 50, 1000); // 动力风扇左 2 - // // pwm_init(FAN_RB_PWM_PIN, 50, 1000); // 动力风扇右 2 - // // system_delay_ms(5000); - // pwm_set_duty(FAN_LS_PWM_PIN, 600); - // pwm_set_duty(FAN_RS_PWM_PIN, 600); - // pwm_set_duty(FAN_LB_PWM_PIN, 600); - // pwm_set_duty(FAN_RB_PWM_PIN, 600); - // while(1); + } /** @@ -121,10 +94,10 @@ void by_pwm_update_duty(uint32_t update_pwm_duty1, uint32_t update_pwm_duty2) */ void by_pwm_power_duty(int32_t bpwm_duty_ls, int32_t bpwm_duty_rs, int32_t bpwm_duty_lb, int32_t bpwm_duty_rb) { - // bpwm_duty_ls = clip_s32(bpwm_duty_ls, 0, 5000); - // bpwm_duty_rs = clip_s32(bpwm_duty_rs, 0, 5000); - // bpwm_duty_lb = clip_s32(bpwm_duty_lb, -3000, 8000); - // bpwm_duty_rb = clip_s32(bpwm_duty_rb, -3000, 8000); + bpwm_duty_ls = clip_s32(bpwm_duty_ls, -9000, 9000); + bpwm_duty_rs = clip_s32(bpwm_duty_rs, -9000, 9000); + bpwm_duty_lb = clip_s32(bpwm_duty_lb, -9000, 9000); + bpwm_duty_rb = clip_s32(bpwm_duty_rb, -9000, 9000); // pwm_duty_ls_g = pwm_duty_ls; // pwm_duty_rs_g = pwm_duty_rs; diff --git a/app/jj_motion.c b/app/jj_motion.c index 91d0a0a..90ae354 100644 --- a/app/jj_motion.c +++ b/app/jj_motion.c @@ -8,7 +8,8 @@ #include "by_imu.h" PID_TypeDef far_angle_pid; -PID_TypeDef angle_pid; +PID_TypeDef str_angle_pid; +PID_TypeDef tur_cal_pid; PID_TypeDef far_gyro_pid; PID_TypeDef speed_pid; // 弯道后面风扇角度环 @@ -38,6 +39,7 @@ float out_angle; float in_pos; float out_pos; float set_pos = 0.0f; +float out_cal; // 弯道角速度环 float gy_Kp0 = 4.50f; float gy_Ki0 = 0.0f; @@ -81,6 +83,8 @@ int32_t pwm_duty_rs; int32_t pwm_duty_lb; int32_t pwm_duty_rb; +int turn_cnt = 0; +int turn_flag = 0; static float myclip_f(float x, float low, float up) { return (x > up ? up : x < low ? low @@ -136,17 +140,14 @@ void sport_motion() set_speed = set_speed1; PID_SetTunings(&far_angle_pid, an_Kp1, an_Ki1, an_Kd1); PID_SetTunings(&far_gyro_pid, gy_Kp1, gy_Ki1, gy_Kd1); - - PID_SetTunings(&angle_pid, po_Kp1, po_Ki1, po_Kd1); } if ((last_state != in_state) && in_state == 1) { // 弯道 bt_printf("to 入弯"); set_speed = set_speed0; cnt3_flag = 1; + turn_flag = 1; PID_SetTunings(&far_angle_pid, an_Kp0, an_Ki0, an_Kd0); PID_SetTunings(&far_gyro_pid, gy_Kp0, gy_Ki0, gy_Kd0); - - PID_SetTunings(&angle_pid, cn_Kp1, cn_Ki1, cn_Kd1); } if ((last_state != in_state) && in_state == 3) { // 圆环 bt_printf("to 圆环"); @@ -154,8 +155,6 @@ void sport_motion() cnt3_flag = 1; PID_SetTunings(&far_angle_pid, yu_Kp0, yu_Ki0, yu_Kd0); PID_SetTunings(&far_gyro_pid, ygy_Kp0, ygy_Ki0, ygy_Kd0); - - PID_SetTunings(&angle_pid, cn_Kp1, cn_Ki1, cn_Kd1); } last_state = in_state; // pid计算 @@ -167,25 +166,26 @@ void sport_motion() in_speed = sport_get_speed(); PID_Compute(&speed_pid); PID_Compute(&far_angle_pid); - PID_Compute(&angle_pid); + PID_Compute(&str_angle_pid); + PID_Compute(&tur_cal_pid); cnt2 = 0; } - if (cnt5 >= 20) { + if (cnt5 >= 50) { cnt5 = 0; } // 输出限幅 if (in_state == 0 || in_state == 2) { - pwm_duty_ls = (int32_t)myclip_f(-out_pos, 0.0f, 5000.f); - pwm_duty_rs = (int32_t)myclip_f(out_pos, 0.0f, 5000.f); - pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 0.0f, 7000.f); - pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 0.0f, 7000.f); + pwm_duty_ls = (int32_t)myclip_f(-out_pos, 0.0f, 6000.f); + pwm_duty_rs = (int32_t)myclip_f(out_pos, 0.0f, 6000.f); + pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 0.0f, 6000.f); + pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 0.0f, 6000.f); } else if (in_state == 1 || in_state == 3) { - pwm_duty_ls = (int32_t)myclip_f(-out_pos, -8000.0f, 8000.f); - pwm_duty_rs = (int32_t)myclip_f(out_pos, -8000.0f, 8000.f); - pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, -7000.0f, 7000.f); - pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, -7000.0f, 7000.f); + pwm_duty_ls = (int32_t)myclip_f(-out_cal, -5000.0f, 6000.f); + pwm_duty_rs = (int32_t)myclip_f(out_cal, -5000.0f, 6000.f); + pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, -6000.0f, 6000.f); + pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, -6000.0f, 6000.f); } by_pwm_power_duty(pwm_duty_ls, pwm_duty_rs, pwm_duty_lb, pwm_duty_rb); @@ -197,7 +197,7 @@ void sport_motion() by_pwm_update_duty(0 + 500, 0 + 500); } else { if ((in_state == 1 || in_state == 3) && cnt3_flag == 1) { - float tt = 100 * fabs(tanf(myclip_f(in_angle, -60.f, 60.f) / 180 * PI)); + float tt = 3 * fabs(in_pos); by_pwm_update_duty(bt_fly + 500 - tt, bt_fly + 500 - tt); cnt3++; if (in_state == 3) { @@ -208,7 +208,7 @@ void sport_motion() cnt3 = 0; } } else { - if (cnt3 >= 400) // 200ms + if (cnt3 >= 500) // 200ms { bt_printf("to 弯道"); cnt3_flag = 2; @@ -232,21 +232,25 @@ void sport_pid_init() PID_SetMode(&far_angle_pid, _PID_MODE_AUTOMATIC); PID_SetSampleTime(&far_angle_pid, 10); PID_SetOutputLimits(&far_angle_pid, -4000.0f, 4000.0f); - - PID(&angle_pid, &in_angle, &out_pos, &set_pos, po_Kp1, po_Ki1, po_Kd1, _PID_P_ON_E, _PID_CD_REVERSE); - PID_SetMode(&angle_pid, _PID_MODE_AUTOMATIC); - PID_SetSampleTime(&angle_pid, 10); - PID_SetOutputLimits(&angle_pid, -8000.0f, 8000.0f); - + // 直道侧面位置环 + PID(&str_angle_pid, &in_angle, &out_pos, &set_pos, po_Kp1, po_Ki1, po_Kd1, _PID_P_ON_E, _PID_CD_REVERSE); + PID_SetMode(&str_angle_pid, _PID_MODE_AUTOMATIC); + PID_SetSampleTime(&str_angle_pid, 10); + PID_SetOutputLimits(&str_angle_pid, -6000.0f, 6000.0f); + // 弯道侧面曲率环 + PID(&tur_cal_pid, &in_pos, &out_cal, &set_pos, cn_Kp1, cn_Ki1, cn_Kd1, _PID_P_ON_E, _PID_CD_DIRECT); + PID_SetMode(&tur_cal_pid, _PID_MODE_AUTOMATIC); + PID_SetSampleTime(&tur_cal_pid, 10); + PID_SetOutputLimits(&tur_cal_pid, -6000.0f, 6000.0f); /* 角速度控制 */ PID(&far_gyro_pid, &in_gyro, &out_gyro, &out_angle, gy_Kp1, gy_Ki1, gy_Kd1, _PID_P_ON_E, _PID_CD_REVERSE); // m是增量 PID_SetMode(&far_gyro_pid, _PID_MODE_AUTOMATIC); PID_SetSampleTime(&far_gyro_pid, 10); - PID_SetOutputLimits(&far_gyro_pid, -7000.0f, 7000.0f); + PID_SetOutputLimits(&far_gyro_pid, -6000.0f, 6000.0f); /* 速度控制 */ PID(&speed_pid, &in_speed, &out_speed, &set_speed, sp_Kp, sp_Ki, sp_Kd, _PID_P_ON_M, _PID_CD_DIRECT); PID_SetMode(&speed_pid, _PID_MODE_AUTOMATIC); PID_SetSampleTime(&speed_pid, 10); - PID_SetOutputLimits(&speed_pid, -4000.0f, 6000.0f); + PID_SetOutputLimits(&speed_pid, -2000.0f, 5000.0f); } diff --git a/app/jj_motion.h b/app/jj_motion.h index e962f3a..b148c3f 100644 --- a/app/jj_motion.h +++ b/app/jj_motion.h @@ -12,6 +12,7 @@ extern float an_Kd1; extern float in_angle; extern float set_angle; extern float out_angle; +extern float out_cal; extern float cn_Kp1; extern float cn_Ki1; extern float cn_Kd1; diff --git a/app/main.c b/app/main.c index ab7784b..61812bc 100644 --- a/app/main.c +++ b/app/main.c @@ -35,7 +35,11 @@ #include "by_frame.h" #include "by_rt_button.h" #include "by_fan_control.h" - +static float myclip_f(float x, float low, float up) +{ + return (x > up ? up : x < low ? low + : x); +} int main(void) { TYPE_UNION test_data[BY_FRAME_DATA_NUM]; @@ -66,7 +70,7 @@ int main(void) Page_Run(); by_frame_parse(&test_data[0].u32); jj_bt_run(); - in_pos = test_data[1].f32; + in_pos = myclip_f(1000.f* test_data[1].f32,-50.f,50.f); in_angle = test_data[0].f32; in_state = test_data[2].u8[0]; in_stop = test_data[2].u8[1]; diff --git a/app/page/page_dparam.c b/app/page/page_dparam.c index 6e2fd5b..3e5b242 100644 --- a/app/page/page_dparam.c +++ b/app/page/page_dparam.c @@ -48,8 +48,8 @@ static void Loop() ips200_show_float(80, 20, out_speed, 4, 1); ips200_show_string(0, 40, "angle"); ips200_show_float(80, 40, in_angle, 4, 1); - ips200_show_string(0, 60, "accz"); - ips200_show_float(80, 60, (float)imu660ra_acc_z, 6, 1); + ips200_show_string(0, 60, "cal"); + ips200_show_float(80, 60, in_pos, 3, 5); ips200_show_string(0, 80, "accy"); ips200_show_float(80, 80, (float)imu660ra_gyro_z, 4, 2); ips200_show_string(0, 100, "speed"); @@ -60,15 +60,17 @@ 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, "outcal"); + ips200_show_float(80, 180, out_cal, 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); + ips200_show_string(100, 0, "ls"); + ips200_show_float(180, 0, pwm_duty_ls, 4, 1); + ips200_show_string(100, 20, "rs"); + ips200_show_float(180, 20, pwm_duty_rs, 4, 1); + ips200_show_string(100, 40, "lb"); + ips200_show_float(180, 40, pwm_duty_lb, 4, 1); + ips200_show_string(100, 60, "rb"); + ips200_show_float(180, 60, pwm_duty_rb, 4, 1); } /** * @brief 页面事件