完赛的一个版本

app/jj_motion.c

@@ -2,7 +2,7 @@
 #include <math.h>
 #include "zf_common_headfile.h"
 #include "pid.h"
-
+#include "jj_voltage.h"
 #include "jj_blueteeth.h"
 #include "by_fan_control.h"
 #include "by_imu.h"
@@ -88,6 +88,7 @@ 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;
 
 /*
 0:无状态
@@ -123,6 +124,7 @@ void sport_motion()
 
     imu660ra_get_gyro();
     imu660ra_get_acc();
+    get_vol();
     in_gyro = imu660ra_gyro_z;
     // 抖动停车
     if (imu660ra_acc_z <= 600) {
@@ -149,6 +151,7 @@ void sport_motion()
         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);
@@ -165,6 +168,7 @@ void sport_motion()
         if ((last_state != in_state) && in_state == 3) { // 圆环
             bt_printf("to 圆环");
             set_speed = set_speed2;
+
             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);
@@ -181,10 +185,11 @@ void sport_motion()
             cnt2 = 0;
         }
         if (cnt5 >= 20) {
-            PID_Compute(&speed_pid);
+
             PID_Compute(&far_angle_pid);
             PID_Compute(&str_angle_pid);
             PID_Compute(&tur_cal_pid);
+            PID_Compute(&speed_pid);
             cnt5 = 0;
         }
         // 输出限幅
@@ -234,8 +239,12 @@ void sport_motion()
                     cnt3      = 0;
                 }
             }
-            float tt = 6 * fabs(in_pos);
-            by_pwm_update_duty(bt_fly + 500 - tt, bt_fly + 500 - tt);
+            if (in_speed > set_speed - 100) {
+                qd_down = 6 * fabs(in_pos);
+            } else {
+                qd_down = 0;
+            }
+            by_pwm_update_duty(bt_fly + 500 - qd_down, bt_fly + 500 - qd_down);
         } else {
             if (in_state == 4) {
                 by_pwm_update_duty(bt_fly + 515, bt_fly + 515);
@@ -277,7 +286,7 @@ 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, -5500.0f, 5500.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);