diff --git a/app/by_fan_control.c b/app/by_fan_control.c index 987d8ee..d9c9bd6 100644 --- a/app/by_fan_control.c +++ b/app/by_fan_control.c @@ -6,21 +6,10 @@ #define FAN_LL_PWM_PIN TIM8_PWM_MAP1_CH1_B6 // 左升力风扇 #define FAN_RL_PWM_PIN TIM8_PWM_MAP1_CH2_B7 // 右升力风扇 // M4 -#define FAN_RB_PWM_A_PIN TIM10_PWM_MAP3_CH2_D3 // 后 -#define FAN_RB_PWM_B_PIN TIM10_PWM_MAP3_CH4_D7 // you后 -// M1 -#define FAN_LB_PWM_A_PIN TIM4_PWM_MAP1_CH1_D12 // zuo后 -#define FAN_LB_PWM_B_PIN TIM4_PWM_MAP1_CH3_D14// 后 -// M3 -// #define FAN_RS_PWM_B_PIN TIM10_PWM_MAP3_CH1_D1 // 右侧 -// #define FAN_RS_PWM_A_PIN TIM10_PWM_MAP3_CH3_D5 // you侧 -// // M2 -// #define FAN_LS_PWM_B_PIN TIM4_PWM_MAP1_CH4_D15 // 左侧 -// #define FAN_LS_PWM_A_PIN TIM4_PWM_MAP1_CH2_D13 // 左侧 - -// +#define FAN_RB_PWM_PIN TIM10_PWM_MAP3_CH4_D7 +#define FAN_LB_PWM_PIN TIM4_PWM_MAP1_CH2_D13 #define FAN_LS_PWM_PIN TIM1_PWM_MAP0_CH4_A11 -#define FAN_RS_PWM_PIN TIM1_PWM_MAP0_CH1_A8 +#define FAN_RS_PWM_PIN TIM1_PWM_MAP0_CH1_A8 int32_t pwm_duty_ls_g; int32_t pwm_duty_rs_g; int32_t pwm_duty_lb_g; @@ -53,35 +42,8 @@ void by_pwm_init(void) pwm_init(FAN_RL_PWM_PIN, 50, 500); pwm_init(FAN_LS_PWM_PIN, 50, 500); pwm_init(FAN_RS_PWM_PIN, 50, 500); -#if FIX_DRIVE == 1 - pwm_init(FAN_LS_PWM_A_PIN, 20000, 7000); - pwm_init(FAN_LS_PWM_B_PIN, 20000, 7000); - pwm_init(FAN_RS_PWM_A_PIN, 20000, 7000); - pwm_init(FAN_RS_PWM_B_PIN, 20000, 7000); - pwm_init(FAN_LB_PWM_A_PIN, 20000, 7000); - pwm_init(FAN_LB_PWM_B_PIN, 20000, 7000); - pwm_init(FAN_RB_PWM_A_PIN, 20000, 7000); - pwm_init(FAN_RB_PWM_B_PIN, 20000, 7000); - while (1); -#elif FIX_DRIVE == 2 - // pwm_init(FAN_LS_PWM_A_PIN, 30000, 1000); - // pwm_init(FAN_LS_PWM_B_PIN, 30000, 0); - // pwm_init(FAN_RS_PWM_A_PIN, 30000, 1000); - // pwm_init(FAN_RS_PWM_B_PIN, 30000, 0); - pwm_init(FAN_LB_PWM_A_PIN, 30000, 1000); - pwm_init(FAN_LB_PWM_B_PIN, 30000, 0); - pwm_init(FAN_RB_PWM_A_PIN, 30000, 1000); - pwm_init(FAN_RB_PWM_B_PIN, 30000, 0); - while (1); -#endif - // pwm_init(FAN_LS_PWM_A_PIN, 40000, 0); - // pwm_init(FAN_LS_PWM_B_PIN, 40000, 0); - // pwm_init(FAN_RS_PWM_A_PIN, 40000, 0); - // pwm_init(FAN_RS_PWM_B_PIN, 40000, 0); - pwm_init(FAN_LB_PWM_A_PIN, 40000, 0); - pwm_init(FAN_LB_PWM_B_PIN, 40000, 0); - pwm_init(FAN_RB_PWM_A_PIN, 40000, 0); - pwm_init(FAN_RB_PWM_B_PIN, 40000, 0); + pwm_init(FAN_LB_PWM_PIN, 300, 300); + pwm_init(FAN_RB_PWM_PIN, 300, 300); } /** @@ -110,46 +72,10 @@ void by_pwm_power_duty(int32_t bpwm_duty_ls, int32_t bpwm_duty_rs, int32_t bpwm_ { bpwm_duty_ls = clip_s32(bpwm_duty_ls, 500, 1000); bpwm_duty_rs = clip_s32(bpwm_duty_rs, 500, 1000); - // if(1000==bpwm_duty_ls)bpwm_duty_rs=600; - // if(1000==bpwm_duty_rs)bpwm_duty_ls=600; - bpwm_duty_lb = clip_s32(bpwm_duty_lb, 0, 7000); - bpwm_duty_rb = clip_s32(bpwm_duty_rb, 0, 7000); + bpwm_duty_lb = clip_s32(bpwm_duty_lb, 2500, 6000); + bpwm_duty_rb = clip_s32(bpwm_duty_rb, 2500, 6000); pwm_set_duty(FAN_LS_PWM_PIN, bpwm_duty_ls); pwm_set_duty(FAN_RS_PWM_PIN, bpwm_duty_rs); - // 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; - - // if (bpwm_duty_ls >= 0) { - - // pwm_set_duty(FAN_LS_PWM_B_PIN, 0); - // } else { - // pwm_set_duty(FAN_LS_PWM_A_PIN, 0); - // pwm_set_duty(FAN_LS_PWM_B_PIN, -1 * pwm_duty_ls); - // } - - // if (bpwm_duty_rs >= 0) { - - // pwm_set_duty(FAN_RS_PWM_B_PIN, 0); - // } else { - // pwm_set_duty(FAN_RS_PWM_A_PIN, 0); - // pwm_set_duty(FAN_RS_PWM_B_PIN, -1 * pwm_duty_rs); - // } - - if (bpwm_duty_lb >= 0) { - pwm_set_duty(FAN_LB_PWM_A_PIN, bpwm_duty_lb); - pwm_set_duty(FAN_LB_PWM_B_PIN, 0); - } else { - pwm_set_duty(FAN_LB_PWM_A_PIN, 0); - pwm_set_duty(FAN_LB_PWM_B_PIN, -1 * bpwm_duty_lb); - } - - if (bpwm_duty_rb >= 0) { - pwm_set_duty(FAN_RB_PWM_A_PIN, bpwm_duty_rb); - pwm_set_duty(FAN_RB_PWM_B_PIN, 0); - } else { - pwm_set_duty(FAN_RB_PWM_A_PIN, 0); - pwm_set_duty(FAN_RB_PWM_B_PIN, -1 * bpwm_duty_rb); - } -} + pwm_set_duty(FAN_LB_PWM_PIN, bpwm_duty_lb); + pwm_set_duty(FAN_RB_PWM_PIN, bpwm_duty_rb); +} \ No newline at end of file diff --git a/app/jj_blueteeth.c b/app/jj_blueteeth.c index 7c06556..97b786a 100644 --- a/app/jj_blueteeth.c +++ b/app/jj_blueteeth.c @@ -6,7 +6,7 @@ uint8_t bt_buffer; // 接收字符存入 uint8_t bt_run_flag = 0; uint8_t bt_fly_flag = 0; uint32_t bt_run = 0; -float bt_fly = 250.f; +float bt_fly = 160.f; enum bt_order { Fly_ctrl = 0x01, // 起飞转换 Fly_up = 0x02, // 起飞程度增加 diff --git a/app/jj_motion.c b/app/jj_motion.c index 770ebab..57f1f95 100644 --- a/app/jj_motion.c +++ b/app/jj_motion.c @@ -13,43 +13,65 @@ PID_TypeDef pos_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 = 54.0f; -float an_Ki0 = 0.0f; -float an_Kd0 = 0.0f; +/////////////////////////////////////////// // 直道后面风扇角度环 -float an_Kp1 = 45.0f; +float an_Kp1 = 50.0f; float an_Ki1 = 0.0f; float an_Kd1 = 0.0f; +// 直道角速度环 +float gy_Kp1 = 6.0f; +float gy_Ki1 = 0.0f; +float gy_Kd1 = 0.0f; +// 直道侧面风扇角度环 +float po_Kp1 = 13.0f; +float po_Ki1 = 0.0f; +float po_Kd1 = 0.0f; +//////////////////////////////////// // 避障后面角度环 float za_Kp = 50.f; float za_Ki = 0; float za_Kd = 0; -float zg_Kp = 7.f; +float zg_Kp = 3.f; float zg_Ki = 0; float zg_Kd = 0; -float zp_Kp = 300.f; +float zp_Kp = 30.f; float zp_Ki = 0; float zp_Kd = 0; +/////////////////////////////////////// +// 弯道后面风扇角度环 +float an_Kp0 = 60.0f; +float an_Ki0 = 0.0f; +float an_Kd0 = 0.0f; + +// 弯道角速度环 +float gy_Kp0 = 10.80f; +float gy_Ki0 = 0.0f; +float gy_Kd0 = 0.0f; + +// 弯道侧面风扇角度环 +float cn_Kp1 = 25.0f; +float cn_Ki1 = 0.f; +float cn_Kd1 = 0.0f; +//////////////////////////////////////// // 圆环后面风扇角度环 float yu_Kp0 = 60.0f; float yu_Ki0 = 0.0f; float yu_Kd0 = 0.0f; -// 弯道侧面风扇角度环 -float cn_Kp1 = 400.0f; -float cn_Ki1 = 0.f; -float cn_Kd1 = 0.0f; -// 直道侧面风扇角度环 -float po_Kp1 = 200.0f; -float po_Ki1 = 0.0f; -float po_Kd1 = 0.0f; +// 圆环角速度环 +float ygy_Kp0 = 8.40f; +float ygy_Ki0 = 0.0f; +float ygy_Kd0 = 0.0f; +// 速度环 +float sp_Kp = 2.0f; +float sp_Ki = 4.f; +float sp_Kd = 0.0f; +//////////////////////////////////////////////////// +// 当前和输入量 +float in_gyro; +float out_gyro; // 期望和当前量 float in_angle; float set_angle = 0.0f; @@ -57,36 +79,18 @@ float out_angle; float in_pos; float out_pos; float set_pos = 0.0f; -// 弯道角速度环 -float gy_Kp0 = 8.80f; -float gy_Ki0 = 0.0f; -float gy_Kd0 = 0.0f; -// 直道角速度环 -float gy_Kp1 = 7.0f; -float gy_Ki1 = 0.0f; -float gy_Kd1 = 0.0f; -// 圆环角速度环 -float ygy_Kp0 = 6.0f; -float ygy_Ki0 = 0.0f; -float ygy_Kd0 = 0.0f; -// 当前和输入量 -float in_gyro; -float out_gyro; // float set_gyro = 0.0f; -// 速度环 -float sp_Kp = 10.0f; -float sp_Ki = 60.f; -float sp_Kd = 0.0f; + float in_speed; float out_speed; float set_speed; -float set_speed0 = 900.0f; -float set_speed1 = 1200.0f; -float set_speed2 = 700.f; -float set_speed3 = 900.f; -float set_speed4 = 500.f; +float set_speed0 = 657.0f; +float set_speed1 = 1111.0f; +float set_speed2 = 815.f; +float set_speed3 = 666.f; +float set_speed4 = 720.f; int cnt1 = 0; int cnt2 = 0; int cnt3 = 0; @@ -95,7 +99,8 @@ int cnt5 = 0; int cnt6 = 0; int cnt7 = 0; int cnt8 = 0; -uint8_t cnt3_flag = 0; +int if_start = 0; +float shock_stop=0; uint8_t in_state = 0; uint8_t in_stop = 0; uint8_t last_state = 0; @@ -149,17 +154,17 @@ void sport_motion() } else { cnt7 = 0; } - if (cnt7 >= 100) { + if (cnt7 >= 100&&shock_stop==1) { bt_fly_flag = bt_run_flag = 0; bt_printf("ting"); } - // 斑马线停车 - if (1 == in_stop) { + // 斑马线停车,摄像头识别 + if (1 == in_stop && if_start == 1) { stop_flag = 1; } if (stop_flag == 1) { cnt4++; - if (cnt4 >= 200) { + if (cnt4 >= 100) { bt_fly_flag = bt_run_flag = 0; } } @@ -168,11 +173,15 @@ void sport_motion() cnt1++; cnt2++; cnt5++; - + if (if_start == 0) { + cnt8++; + if (cnt8 >= 10000) { + if_start = 1; + } + } // pid参数切换 if ((last_state != in_state) && (in_state == 0 || in_state == 2)) { // 直道 - cnt3_flag = 1; - cnt3 = 0; + bt_printf("to 直道"); set_speed = set_speed1; PID_SetTunings(&far_angle_pid, an_Kp1, an_Ki1, an_Kd1); @@ -182,7 +191,7 @@ void sport_motion() if ((last_state != in_state) && in_state == 1) { // 弯道 bt_printf("to 入弯"); set_speed = set_speed0; - cnt3_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(&pos_pid, cn_Kp1, cn_Ki1, cn_Kd1); @@ -193,7 +202,7 @@ void sport_motion() set_speed = set_speed2; else set_speed = set_speed3; - 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(&pos_pid, cn_Kp1, cn_Ki1, cn_Kd1); @@ -201,7 +210,7 @@ void sport_motion() if ((last_state != in_state) && in_state == 5) { // bt_printf("to 障碍"); set_speed = set_speed4; - cnt3_flag = 1; + PID_SetTunings(&far_angle_pid, za_Kp, za_Ki, za_Kd); PID_SetTunings(&far_gyro_pid, zg_Kp, zg_Ki, zg_Kd); PID_SetTunings(&pos_pid, zp_Kp, zp_Ki, zp_Kd); @@ -226,10 +235,10 @@ void sport_motion() // 并级pid，限幅 pwm_duty_ls = (int32_t)myclip_f(300 - out_pos, 100.0f, 500.f); pwm_duty_rs = (int32_t)myclip_f(300 + out_pos, 100.0f, 500.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_lb = (int32_t)myclip_f(out_speed + out_gyro, 300.0f, 3000.f); + pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 300.0f, 3000.f); - by_pwm_power_duty(500 + pwm_duty_ls, 500 + pwm_duty_rs, pwm_duty_lb, pwm_duty_rb); + by_pwm_power_duty(500 + pwm_duty_ls, 500 + pwm_duty_rs, 3600 + pwm_duty_lb, 3600 + pwm_duty_rb); } else { by_pwm_power_duty(0, 0, 0, 0); @@ -238,56 +247,16 @@ void sport_motion() if (bt_fly_flag == 0) { by_pwm_update_duty(500, 500); } else { - if ((in_state == 1 || in_state == 3 || in_state == 5) && cnt3_flag == 1) { - cnt3++; + if (in_state == 1) { - if (in_state == 3) { - if (cnt3 >= 1000) // 200ms - { - bt_printf("to 环内"); - cnt3_flag = 2; - cnt3 = 0; - } - if (in_speed > set_speed) { - qd_down = 25; - } else if (in_speed > set_speed - 100) { - qd_down = 10; - } else { - qd_down = 0; - } - } else if (in_state == 1) { - if (cnt3 >= 300) // 200ms - { - bt_printf("to 弯道"); - cnt3_flag = 2; - cnt3 = 0; - } - if (in_speed > set_speed) - qd_down = (in_speed - set_speed) * 0.5 + 40; - else - qd_down = -10; - } + qd_down = 20; - by_pwm_update_duty(bt_fly + 500 - qd_down, bt_fly + 500 - qd_down); - } else { - if (in_state == 1) { // 消除s弯降气垫 - if (in_speed >= set_speed / 1.2) { - qd_down = 40; - } else if (in_speed >= set_speed / 1.5) { - qd_down = 25; - } else if (in_speed >= set_speed / 2) { - qd_down = 10; - }else { - qd_down = 0; - } - - } else { - qd_down = 0; - } - - by_pwm_update_duty(bt_fly + 500 - qd_down, bt_fly + 500 - qd_down); + } else if (in_state == 4) { + qd_down = 20; } + + by_pwm_update_duty(bt_fly + 500 - qd_down, bt_fly + 500 - qd_down); } } @@ -312,11 +281,11 @@ void sport_pid_init() 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, -4800.0f, 4800.0f); + PID_SetOutputLimits(&far_gyro_pid, -1200.0f, 1200.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, -0.0f, 6000.0f); + PID_SetOutputLimits(&speed_pid, -0.0f, 1800.0f); } diff --git a/app/jj_motion.h b/app/jj_motion.h index 8d1a125..2d86754 100644 --- a/app/jj_motion.h +++ b/app/jj_motion.h @@ -57,6 +57,7 @@ extern float set_speed2; extern float set_speed3; extern float set_speed; extern float set_speed4; +extern float shock_stop; extern int32_t pwm_duty_ls; extern int32_t pwm_duty_rs; extern int32_t pwm_duty_lb; diff --git a/app/jj_param.c b/app/jj_param.c index 9293dbc..943bc90 100644 --- a/app/jj_param.c +++ b/app/jj_param.c @@ -76,6 +76,8 @@ void jj_param_eeprom_init(void) PARAM_REG(zhpo_Kd, &zp_Kd, EFLOAT, 1, "po_D1:"); PARAM_REG(param_set_speed4, &set_speed4, EFLOAT, 1, "rate4:"); + + PARAM_REG(shock, &shock_stop, EFLOAT, 1, "shock:"); jj_param_read(); // 注冊 } /** diff --git a/app/jj_param.h b/app/jj_param.h index 6d27305..2d892ab 100644 --- a/app/jj_param.h +++ b/app/jj_param.h @@ -76,6 +76,8 @@ typedef enum { zhpo_Ki, zhpo_Kd, + shock, + param_set_speed4, DATA_IN_FLASH_NUM,