diff --git a/app/jj_motion.c b/app/jj_motion.c index f85e94e..6bc1896 100644 --- a/app/jj_motion.c +++ b/app/jj_motion.c @@ -27,19 +27,6 @@ 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 = 3.f; -float zg_Ki = 0; -float zg_Kd = 0; - -float zp_Kp = 30.f; -float zp_Ki = 0; -float zp_Kd = 0; -/////////////////////////////////////// // 弯道后面风扇角度环 float an_Kp0 = 60.0f; float an_Ki0 = 0.0f; @@ -55,7 +42,19 @@ float cn_Kp1 = 25.0f; float cn_Ki1 = 0.f; float cn_Kd1 = 0.0f; //////////////////////////////////////// +// 避障后面角度环 +float za_Kp = 50.f; +float za_Ki = 0; +float za_Kd = 0; +float zg_Kp = 3.f; +float zg_Ki = 0; +float zg_Kd = 0; + +float zp_Kp = 30.f; +float zp_Ki = 0; +float zp_Kd = 0; +/////////////////////////////////////// // 圆环后面风扇角度环 float yu_Kp0 = 60.0f; float yu_Ki0 = 0.0f; @@ -64,6 +63,8 @@ float yu_Kd0 = 0.0f; float ygy_Kp0 = 8.40f; float ygy_Ki0 = 0.0f; float ygy_Kd0 = 0.0f; +// 圆环侧面角度环 +float yuc_Kp; // 速度环 float sp_Kp = 2.0f; float sp_Ki = 4.f; @@ -109,8 +110,10 @@ int32_t pwm_duty_ls; int32_t pwm_duty_rs; int32_t pwm_duty_lb; int32_t pwm_duty_rb; -float qd_down = 0.0f; -float last_gy = 0.0f; +float qd_down = 0.0f; +float last_gy = 0.0f; +int32_t last_lb = 0; +int32_t last_rb = 0; /* 0:无状态 1:弯道 @@ -147,10 +150,10 @@ void sport_motion() get_vol(); in_gyro = imu660ra_gyro_z; - if (fabs(out_gyro) < 900) { - PID_SetOutputLimits(&speed_pid, -0.0f, 3000.0f); + if (fabs(out_gyro) < 700) { + PID_SetOutputLimits(&speed_pid, -0.0f, 2500.0f); } else { - PID_SetOutputLimits(&speed_pid, -0.0f, 2100.0f); + PID_SetOutputLimits(&speed_pid, -0.0f, 1800.0f); } ////////////////////////////////////////停车任务/////////////////////////////////////////////// // 抖动停车 @@ -178,11 +181,10 @@ void sport_motion() } // 出界停车 if (2 == in_stop) { - bt_printf("over"); + bt_printf("out"); bt_fly_flag = bt_run_flag = 0; } - /////////////////////////////////////////////////////////////////////////////////////// - /* 动力风扇设置 */ + //////////////////////////////////////////////// 动力风扇设置 *///////////////////////////////////////////////////////// if (1 == bt_run_flag) { cnt1++; cnt2++; @@ -219,7 +221,7 @@ void sport_motion() 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); + PID_SetTunings(&pos_pid, yuc_Kp, cn_Ki1, cn_Kd1); } if ((last_state != in_state) && in_state == 5) { // bt_printf("to 障碍"); @@ -249,28 +251,30 @@ void sport_motion() // 并级pid,限幅 pwm_duty_ls = (int32_t)myclip_f(320 - out_pos, 140.0f, 500.f); pwm_duty_rs = (int32_t)myclip_f(320 + out_pos, 140.0f, 500.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); + pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 1000.0f, 3000.f); + pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 1000.0f, 3000.f); + if (pwm_duty_lb - last_lb > 1000) { + pwm_duty_lb = (pwm_duty_lb + last_lb) / 2; + } + if (pwm_duty_rb - last_rb > 1000) { + pwm_duty_rb = (pwm_duty_rb + last_rb) / 2; + } - by_pwm_power_duty(500 + pwm_duty_ls, 500 + pwm_duty_rs, 3600 + pwm_duty_lb, 3600 + pwm_duty_rb); + last_rb = pwm_duty_rb; + last_lb = pwm_duty_lb; + by_pwm_power_duty(500 + pwm_duty_ls, 500 + pwm_duty_rs, 3000 + pwm_duty_lb, 3000 + pwm_duty_rb); } else { by_pwm_power_duty(0, 0, 0, 0); } - /* 升力风扇设置 */ + ///////////////////////////////////////////////////* 升力风扇设置 */ ///////////////////////////////////////////////////////////////////////////////////////////////// if (bt_fly_flag == 0) { by_pwm_update_duty(500, 500); } else { - - if (in_state == 1) { - - qd_down = 20; - - } else if (in_state == 4) { - qd_down = 20; - } - - by_pwm_update_duty(bt_fly + 500 - qd_down, bt_fly + 500 - qd_down); + by_pwm_update_duty(bt_fly + 500, bt_fly + 500); + // 防止起步拉满 + if (bt_run_flag == 0) + by_pwm_power_duty(0, 0, 4000, 4000); } } @@ -284,7 +288,7 @@ void sport_pid_init() 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_SetOutputLimits(&far_angle_pid, -4000.0f, 4000.0f); // 侧面位置环 PID(&pos_pid, &in_angle, &out_pos, &set_pos, po_Kp1, po_Ki1, po_Kd1, _PID_P_ON_E, _PID_CD_REVERSE); PID_SetMode(&pos_pid, _PID_MODE_AUTOMATIC); @@ -301,5 +305,5 @@ void sport_pid_init() 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, 2100.0f); + PID_SetOutputLimits(&speed_pid, -0.0f, 1800.0f); } diff --git a/app/jj_motion.h b/app/jj_motion.h index 5c0fb95..b7b69ac 100644 --- a/app/jj_motion.h +++ b/app/jj_motion.h @@ -59,6 +59,7 @@ extern float set_speed; extern float set_speed4; extern float shock_stop; extern float last_angle; +extern float yuc_Kp; 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 943bc90..0d1f4e4 100644 --- a/app/jj_param.c +++ b/app/jj_param.c @@ -27,7 +27,7 @@ void jj_param_eeprom_init(void) PARAM_REG(gyro_Kd0, &gy_Kd0, EFLOAT, 1, "gy_D0:"); PARAM_REG(can_Kp1, &cn_Kp1, EFLOAT, 1, "cn_P1:"); // 注冊 - PARAM_REG(can_Ki1, &cn_Ki1, EFLOAT, 1, "cn_I1:"); // 注冊 + PARAM_REG(can_Ki1, &yuc_Kp, EFLOAT, 1, "yu_P1:"); // 注冊 PARAM_REG(can_Kd1, &cn_Kd1, EFLOAT, 1, "cn_D1:"); PARAM_REG(fly_pwm, &bt_fly, EFLOAT, 1, "fly:");