feat: 完成控制代码编写

app/jj_motion.c

@@ -1,44 +1,80 @@
 #include "jj_motion.h"
 #include "by_fan_control.h"
 #include "by_imu.h"
-#include "../3rd-lib/PID-Library/pid.h"
 #include "jj_blueteeth.h"
-#include "gl_common.h"
-PID_TypeDef angle_pid;
-PID_TypeDef gyro_z_pid;
-PID_TypeDef img_x_pid;
+#include "../3rd-lib/PID-Library/pid.h"
+#include "zf_driver_encoder.h"
+
+PID_TypeDef far_angle_pid;
+PID_TypeDef far_gyro_pid;
+PID_TypeDef near_pos_pid;
+PID_TypeDef speed_pid;
+
 float an_Kp = 8.0f;
-float an_Ki = 0.0f;
+float an_Ki = 1.0f;
 float an_Kd = 2.0f;
-float im_Kp = 1.0f;
-float im_Ki = 0.0f;
-float im_Kd = 0.0f;
-float set_x = 0.0f;
-float img_x = 40.0f;
-float yaw0  = 0;
-float out_M = 0;
-float out_yaw;
-uint32_t cnt1 = 0;
+float in_angle;
+float set_angle = 0.0f;
+float out_angle;
+
+float gy_Kp = 1.0f;
+float gy_Ki = 0.0f;
+float gy_Kd = 0.0f;
+float in_gyro;
+float out_gyro;
+//float set_gyro = 0.0f;
+
+float po_Kp = 1.0f;
+float po_Ki = 0.0f;
+float po_Kd = 0.0f;
+float in_pos;
+float out_pos;
+float set_pos = 0.0f;
+
+float sp_Kp = 1.0f;
+float sp_Ki = 0.0f;
+float sp_Kd = 0.0f;
+float in_speed;
+float out_speed;
+float set_speed = 0.0f;
+
 /**
  * @brief
  *
- * @param fy y轴的加速度
  */
-void sport_motion2(uint32_t fy)
+int cnt1 = 0;
+void sport_motion(void)
 {
+
     cnt1++;
-    yaw0 = imu660ra_gyro_z;
-    if (cnt1 >= 50) {
-        PID_Compute(&angle_pid);
+    in_gyro  = imu660ra_gyro_z;                                       // 陀螺仪输入
+    in_angle = 0;                                                     // 图像远端输入
+    in_pos   = 0;                                                     // 图像近端输入
+    in_speed = encoder_get_count(TIM5_ENCOEDER) / 1024 / 0.01 * 0.25; // 速度输入,m/s
+    encoder_clear_count(TIM5_ENCOEDER);                               // 清除计数
+    PID_Compute(&far_gyro_pid);
+
+    if (cnt1 >= 10) {
+        PID_Compute(&far_angle_pid);
+        PID_Compute(&speed_pid);
         cnt1 = 0;
     }
-    PID_Compute(&gyro_z_pid);
+
+    PID_Compute(&near_pos_pid);
+
     if (bt_run_flag == 1) {
-        by_pwm_power_duty((int32_t)(500 + fy + out_M), (int32_t)(500 + fy - out_M),
-                          (int32_t)(500 + fy + out_M), (int32_t)(500 + fy - out_M));
+        by_pwm_power_duty((int32_t)(500 + out_pos + out_gyro),
+                          (int32_t)(500 - out_pos - out_gyro),
+                          (int32_t)(500 + out_speed + out_gyro),
+                          (int32_t)(500 + out_speed - out_gyro));
     } else {
         by_pwm_power_duty(500, 500, 500, 500);
     }
+    if (bt_fly_flag == 0) {
+        bt_fly = 0;
+    }
+
+    by_pwm_update_duty(bt_fly + 500, bt_fly + 500);
 }
 /**
  * @brief 结构体初始化
@@ -47,14 +83,25 @@ void sport_motion2(uint32_t fy)
 void sport_pid_init()
 {
 
-    PID(&angle_pid, &bt_angle, &out_yaw, &set_x, im_Kp, im_Ki, im_Kd, 0, 0);
-    PID(&gyro_z_pid, &yaw0, &out_M, &out_yaw, an_Kp, an_Ki, an_Kd, 1, 1);
-    PID_Init(&angle_pid);
-    PID_Init(&gyro_z_pid);
+    PID(&far_angle_pid, &in_angle, &out_angle, &set_angle, an_Kp, an_Ki, an_Kd, 0, 0);
+    PID(&far_gyro_pid, &in_gyro, &out_gyro, &out_angle, gy_Kp, gy_Ki, gy_Kd, 1, 1);
+    PID_Init(&far_angle_pid);
+    PID_Init(&far_gyro_pid);
 
-    PID_SetOutputLimits(&angle_pid, -500.0f, 500.0f);
-    PID_SetOutputLimits(&gyro_z_pid, -400.0f, 400.0f);
+    PID_SetOutputLimits(&far_angle_pid, -500.0f, 500.0f);
+    PID_SetOutputLimits(&far_gyro_pid, -500.0f, 500.0f);
 
-    PID_SetMode(&gyro_z_pid, 1);
-    PID_SetMode(&angle_pid, 1);
+    PID_SetMode(&far_gyro_pid, 1);
+    PID_SetMode(&far_angle_pid, 1);
+
+    PID(&near_pos_pid, &in_pos, &out_pos, &set_pos, po_Kp, po_Ki, po_Kd, 0, 0);
+    PID(&speed_pid, &in_speed, &out_speed, &out_speed, sp_Kp, sp_Ki, sp_Kd, 1, 1);
+    PID_Init(&near_pos_pid);
+    PID_Init(&speed_pid);
+
+    PID_SetOutputLimits(&near_pos_pid, -500.0f, 500.0f);
+    PID_SetOutputLimits(&speed_pid, -400.0f, 400.0f);
+
+    PID_SetMode(&near_pos_pid, 1);
+    PID_SetMode(&speed_pid, 1);
 }