feat 更新角度闭环相关功能

app/jj_motion.c

@@ -0,0 +1,60 @@
+#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;
+float an_Kp = 8.0f;
+float an_Ki = 0.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;
+/**
+ * @brief
+ *
+ * @param fy y轴的加速度
+ */
+void sport_motion2(uint32_t fy)
+{
+    cnt1++;
+    yaw0 = imu660ra_gyro_z;
+    if (cnt1 >= 50) {
+        PID_Compute(&angle_pid);
+        cnt1 = 0;
+    }
+    PID_Compute(&gyro_z_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));
+    } else {
+        by_pwm_power_duty(500, 500, 500, 500);
+    }
+}
+/**
+ * @brief 结构体初始化
+ *
+ */
+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_SetOutputLimits(&angle_pid, -500.0f, 500.0f);
+    PID_SetOutputLimits(&gyro_z_pid, -400.0f, 400.0f);
+
+    PID_SetMode(&gyro_z_pid, 1);
+    PID_SetMode(&angle_pid, 1);
+}