From f04b21f7381e74639ad3e8c6c96f2c645f740c0b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E4=BA=95=E6=98=8E=E6=B1=9F?= <246462502@qq.com> Date: Sun, 23 Jun 2024 13:11:56 +0800 Subject: [PATCH] =?UTF-8?q?=E8=83=BD=E7=8E=A9=E8=B5=9B=E7=9A=84=E6=9C=89?= =?UTF-8?q?=E5=88=B7=E7=89=88=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/by_fan_control.c | 24 ++++++------- app/jj_motion.c | 81 ++++++++++++++++++++++++++++++-------------- 2 files changed, 68 insertions(+), 37 deletions(-) diff --git a/app/by_fan_control.c b/app/by_fan_control.c index 52f7a47..7137bd5 100644 --- a/app/by_fan_control.c +++ b/app/by_fan_control.c @@ -49,14 +49,14 @@ void by_pwm_init(void) pwm_init(FAN_RL_PWM_PIN, 50, 500); #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_LS_PWM_A_PIN, 30000, 1000); + pwm_init(FAN_LS_PWM_B_PIN, 30000, 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); + pwm_init(FAN_RB_PWM_A_PIN, 30000, 1000); + pwm_init(FAN_RB_PWM_B_PIN, 30000, 1000); while (1); #elif FIX_DRIVE == 2 pwm_init(FAN_LS_PWM_A_PIN, 12000, 1000); @@ -69,14 +69,14 @@ void by_pwm_init(void) pwm_init(FAN_RB_PWM_B_PIN, 12000, 0); while(1); #endif - pwm_init(FAN_LS_PWM_A_PIN, 14000, 0); - pwm_init(FAN_LS_PWM_B_PIN, 14000, 0); - pwm_init(FAN_RS_PWM_A_PIN, 14000, 0); - pwm_init(FAN_RS_PWM_B_PIN, 14000, 0); - pwm_init(FAN_LB_PWM_A_PIN, 14000, 0); - pwm_init(FAN_LB_PWM_B_PIN, 14000, 0); - pwm_init(FAN_RB_PWM_A_PIN, 14000, 0); - pwm_init(FAN_RB_PWM_B_PIN, 14000, 0); + pwm_init(FAN_LS_PWM_A_PIN, 20000, 0); + pwm_init(FAN_LS_PWM_B_PIN, 20000, 0); + pwm_init(FAN_RS_PWM_A_PIN, 20000, 0); + pwm_init(FAN_RS_PWM_B_PIN, 20000, 0); + pwm_init(FAN_LB_PWM_A_PIN, 20000, 0); + pwm_init(FAN_LB_PWM_B_PIN, 20000, 0); + pwm_init(FAN_RB_PWM_A_PIN, 20000, 0); + pwm_init(FAN_RB_PWM_B_PIN, 20000, 0); } /** diff --git a/app/jj_motion.c b/app/jj_motion.c index 1f25b00..950a603 100644 --- a/app/jj_motion.c +++ b/app/jj_motion.c @@ -12,6 +12,11 @@ PID_TypeDef str_angle_pid; PID_TypeDef tur_cal_pid; PID_TypeDef far_gyro_pid; PID_TypeDef speed_pid; +PID_TypeDef cir_pos_pid; +float ci_Kp0 = 200.f; +float ci_Ki0 = 0; +float ci_Kd0 = 0; +float out_ci; // 弯道后面风扇角度环 float an_Kp0 = 40.0f; float an_Ki0 = 0.0f; @@ -68,6 +73,7 @@ float set_speed; float set_speed0 = 800.0f; float set_speed1 = 1100.0f; float set_speed2 = 810.f; +float set_speed3 = 1000.f; int cnt1 = 0; int cnt2 = 0; int cnt3 = 0; @@ -83,8 +89,14 @@ int32_t pwm_duty_rs; int32_t pwm_duty_lb; int32_t pwm_duty_rb; -int turn_cnt = 0; -int turn_flag = 0; +/* +0:无状态 +1:弯道 +2:直道 +3:入环 +4:环内 +5:障碍 +*/ static float myclip_f(float x, float low, float up) { return (x > up ? up : x < low ? low @@ -113,9 +125,10 @@ void sport_motion() imu660ra_get_acc(); in_gyro = imu660ra_gyro_z; // 抖动停车 - if (imu660ra_acc_z <= 1000) { + if (imu660ra_acc_z <= 600) { bt_fly_flag = bt_run_flag = 0; + bt_printf("ting"); } // 斑马线停车 if (1 == in_stop) { @@ -141,11 +154,11 @@ void sport_motion() PID_SetTunings(&far_angle_pid, an_Kp1, an_Ki1, an_Kd1); PID_SetTunings(&far_gyro_pid, gy_Kp1, gy_Ki1, gy_Kd1); } - if ((last_state != in_state) && in_state == 1) { // 弯道 + if ((last_state != in_state) && (in_state == 1 || in_state == 5)) { // 弯道或者障碍物，暂时没加障碍物的分段 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); } @@ -176,16 +189,16 @@ void sport_motion() } // 输出限幅 if (in_state == 0 || in_state == 2) { - pwm_duty_ls = (int32_t)myclip_f(-out_pos, 0.0f, 6500.f); - pwm_duty_rs = (int32_t)myclip_f(out_pos, 0.0f, 6500.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, 0.0f, 7000.f); + pwm_duty_rs = (int32_t)myclip_f(out_pos, 0.0f, 7000.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); + } else if (in_state == 1 || in_state == 3 || in_state == 4 || in_state == 5) { - pwm_duty_ls = (int32_t)myclip_f(-out_cal, 0.0f, 8000.f); - pwm_duty_rs = (int32_t)myclip_f(out_cal, 0.0f, 8000.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); + pwm_duty_ls = (int32_t)myclip_f(-out_cal, 0.0f, 7000.f); + pwm_duty_rs = (int32_t)myclip_f(out_cal, 0.0f, 7000.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); } by_pwm_power_duty(pwm_duty_ls, pwm_duty_rs, pwm_duty_lb, pwm_duty_rb); @@ -196,27 +209,40 @@ void sport_motion() if (bt_fly_flag == 0) { by_pwm_update_duty(0 + 500, 0 + 500); } else { - if ((in_state == 1 || in_state == 3) && cnt3_flag == 1) { - float tt = 3 * fabs(in_pos); - by_pwm_update_duty(bt_fly + 500 - tt, bt_fly + 500 - tt); + if ((in_state == 1 || in_state == 3 || in_state == 5) && cnt3_flag == 1) { + cnt3++; if (in_state == 3) { - if (cnt3 >= 1200) // 200ms + if (cnt3 >= 300) // 200ms { bt_printf("to 环内"); cnt3_flag = 2; cnt3 = 0; } - } else { - if (cnt3 >= 500) // 200ms + } else if (in_state == 1) { + if (cnt3 >= 450) // 200ms { bt_printf("to 弯道"); cnt3_flag = 2; cnt3 = 0; } + } else if (in_state == 5) { + if (cnt3 >= 200) // 200ms + { + bt_printf("to 障碍"); + cnt3_flag = 2; + cnt3 = 0; + } } + float tt = 6 * fabs(in_pos); + by_pwm_update_duty(bt_fly + 500 - tt, bt_fly + 500 - tt); } else { - by_pwm_update_duty(bt_fly + 500, bt_fly + 500); + if (in_state == 4) { + by_pwm_update_duty(bt_fly + 515, bt_fly + 515); + set_speed = set_speed3; + } else { + by_pwm_update_duty(bt_fly + 500, bt_fly + 500); + } } } } @@ -236,21 +262,26 @@ void sport_pid_init() 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, 20); - PID_SetOutputLimits(&str_angle_pid, -6000.0f, 6000.0f); + PID_SetOutputLimits(&str_angle_pid, -7000.0f, 7000.0f); // 弯道侧面曲率环 PID(&tur_cal_pid, &in_angle, &out_cal, &set_pos, cn_Kp1, cn_Ki1, cn_Kd1, _PID_P_ON_E, _PID_CD_REVERSE); PID_SetMode(&tur_cal_pid, _PID_MODE_AUTOMATIC); PID_SetSampleTime(&tur_cal_pid, 20); - PID_SetOutputLimits(&tur_cal_pid, -8000.0f, 8000.0f); + PID_SetOutputLimits(&tur_cal_pid, -7000.0f, 7000.0f); + // 圆环 + PID(&cir_pos_pid, &in_angle, &out_ci, &set_pos, ci_Kp0, ci_Ki0, ci_Kd0, _PID_P_ON_E, _PID_CD_REVERSE); + PID_SetMode(&cir_pos_pid, _PID_MODE_AUTOMATIC); + PID_SetSampleTime(&cir_pos_pid, 20); + PID_SetOutputLimits(&cir_pos_pid, -7000.0f, 7000.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, 20); - PID_SetOutputLimits(&far_gyro_pid, -6000.0f, 6000.0f); + PID_SetOutputLimits(&far_gyro_pid, -5500.0f, 5500.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, 20); - PID_SetOutputLimits(&speed_pid, -2000.0f, 5000.0f); + PID_SetOutputLimits(&speed_pid, -0.0f, 6000.0f); }