pref: 优化控制pid，将角速度环输出加到侧面风扇上

3rd-lib/PID-Library/pid.c

@@ -39,7 +39,6 @@ void PID_Init(PID_TypeDef *uPID)
 	
 	
 }
-
 void PID(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection)
 {
 	/* ~~~~~~~~~~ Set parameter ~~~~~~~~~~ */

3rd-lib/PID-Library/pid.h

@@ -141,7 +141,7 @@ typedef struct
 /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Prototype ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 /* :::::::::::::: Init ::::::::::::: */
 void PID_Init(PID_TypeDef *uPID);
-
+void PID_I_Clear(PID_TypeDef *uPID);
 void PID(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection);
 void PID2(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDCD_TypeDef ControllerDirection);
 

app/by_fan_control.c

@@ -1,3 +1,4 @@
+#include <stdio.h>
 #include "by_fan_control.h"
 #include "zf_common_headfile.h"
 
@@ -8,7 +9,18 @@
 #define FAN_LB_PWM_PIN TIM4_PWM_MAP1_CH2_D13 // 左后动力风扇
 #define FAN_RB_PWM_PIN TIM4_PWM_MAP1_CH1_D12 // 右后动力风扇
 
-uint32_t myclip(uint32_t x, uint32_t low, uint32_t up)
+uint32_t pwm_duty_ls_g;
+uint32_t pwm_duty_rs_g;
+uint32_t pwm_duty_lb_g;
+uint32_t pwm_duty_rb_g;
+
+static uint32_t myclip(uint32_t x, uint32_t low, uint32_t up)
+{
+    return (x > up ? up : x < low ? low
+                                  : x);
+}
+
+static float myclip_f(float x, float low, float up)
 {
     return (x > up ? up : x < low ? low
                                   : x);
@@ -19,10 +31,10 @@ void by_pwm_init(void)
     pwm_init(FAN_LL_PWM_PIN, 50, 500);
     pwm_init(FAN_RL_PWM_PIN, 50, 500);
 
-    pwm_init(FAN_LS_PWM_PIN, 4000, 500);
+    pwm_init(FAN_LS_PWM_PIN, 4000, 0);
-    pwm_init(FAN_RS_PWM_PIN, 4000, 500);
+    pwm_init(FAN_RS_PWM_PIN, 4000, 0);
-    pwm_init(FAN_LB_PWM_PIN, 4000, 500);
+    pwm_init(FAN_LB_PWM_PIN, 4000, 0);
-    pwm_init(FAN_RB_PWM_PIN, 4000, 500);
+    pwm_init(FAN_RB_PWM_PIN, 4000, 0);
     // system_delay_ms(3000);
     // // pwm_init(FAN_LS_PWM_PIN, 50, 1000); // 动力风扇左 1
     // // pwm_init(FAN_RS_PWM_PIN, 50, 1000); // 动力风扇右 1
@@ -60,11 +72,15 @@ void by_pwm_update_duty(uint32_t update_pwm_duty1, uint32_t update_pwm_duty2)
  */
 void by_pwm_power_duty(uint32_t pwm_duty_ls, uint32_t pwm_duty_rs, uint32_t pwm_duty_lb, uint32_t pwm_duty_rb)
 {
+    pwm_duty_ls = myclip(pwm_duty_ls, 0, 5000);
+    pwm_duty_rs = myclip(pwm_duty_rs, 0, 5000);
+    pwm_duty_lb = myclip(pwm_duty_lb, 0, 5000);
+    pwm_duty_rb = myclip(pwm_duty_rb, 0, 5000);
 
-    pwm_duty_ls = myclip(pwm_duty_ls, 500, 3000);
+    pwm_duty_ls_g = pwm_duty_ls;
-    pwm_duty_rs = myclip(pwm_duty_rs, 500, 3000);
+    pwm_duty_rs_g = pwm_duty_rs;
-    pwm_duty_lb = myclip(pwm_duty_lb, 500, 3000);
+    pwm_duty_lb_g = pwm_duty_lb;
-    pwm_duty_rb = myclip(pwm_duty_rb, 500, 3000);
+    pwm_duty_rb_g = pwm_duty_rb;
 
     pwm_set_duty(FAN_LS_PWM_PIN, pwm_duty_ls);
     pwm_set_duty(FAN_RS_PWM_PIN, pwm_duty_rs);

app/by_fan_control.h

@@ -4,8 +4,13 @@
 #include "stdio.h"
 #include "ch32v30x.h"
 
+extern uint32_t pwm_duty_ls_g;
+extern uint32_t pwm_duty_rs_g;
+extern uint32_t pwm_duty_lb_g;
+extern uint32_t pwm_duty_rb_g;
+
 extern void by_pwm_init(void);
-void by_pwm_update_duty(uint32_t update_pwm_duty1,uint32_t update_pwm_duty2);
+extern void by_pwm_update_duty(uint32_t update_pwm_duty1,uint32_t update_pwm_duty2);
-void by_pwm_power_duty(uint32_t power_pwm_duty_l1, uint32_t power_pwm_duty_r1,uint32_t power_pwm_duty_l2, uint32_t power_pwm_duty_r2);
+extern void by_pwm_power_duty(uint32_t power_pwm_duty_l1, uint32_t power_pwm_duty_r1,uint32_t power_pwm_duty_l2, uint32_t power_pwm_duty_r2);
 
 #endif

app/jj_blueteeth.c

@@ -14,6 +14,7 @@ enum bt_order {
     Speed_up   = 0x04, // 加速
     Speed_down = 0x05, // 减速
     Speed_ctrl = 0x06, // 速度开关
+    Reset,
 };
 /**
  * @brief  蓝牙初始化
@@ -50,6 +51,9 @@ void jj_bt_run()
             case Speed_down:
                 bt_run -= 50;
                 break;
+            case Reset:
+                NVIC_SystemReset();
+                break;
             default:
                 break;
         }

app/jj_motion.c

@@ -42,6 +42,13 @@ float set_speed = 0.0f;
 
 int cnt1 = 0;
 int cnt2 = 0;
+
+static float myclip_f(float x, float low, float up)
+{
+    return (x > up ? up : x < low ? low
+                                  : x);
+}
+
 float sport_get_speed(void)
 {
 #define ALPHA (0.97f)
@@ -60,56 +67,51 @@ float sport_get_speed(void)
 
 void sport_motion(void)
 {
-
-    cnt1++;
-    cnt2++;
-
-    imu660ra_get_gyro();
-    in_gyro = imu660ra_gyro_z; // 陀螺仪输入
-    // in_angle = 0;                                                     // 图像远端输入
-    // in_pos   = 0;                                                     // 图像近端输入
-    //  清除计数
-    PID_Compute(&far_gyro_pid);
-
-    if (cnt1 >= 10) {
-        in_speed = -1 * sport_get_speed();
-        PID_Compute(&speed_pid);
-
-        PID_Compute(&near_pos_pid);
-
-        cnt1 = 0;
-    }
-
-    if (cnt2 >= 20) {
-        cnt2 = 0;
-        PID_Compute(&far_angle_pid);
-    }
-
     /* 动力风扇设置 */
     if (1 == bt_run_flag) {
-        if (out_gyro > 0) {
+        cnt1++;
-            by_pwm_power_duty((int32_t)(500 - 1 * out_pos + 0 * out_gyro),
+        cnt2++;
-                              (int32_t)(500 + 1 * out_pos - 0 * out_gyro),
-                              (int32_t)(500 + out_speed + out_gyro),
-                              (int32_t)(500 + out_speed - out_gyro));
 
-        } else if (out_gyro <= 0) {
+        imu660ra_get_gyro();
-            by_pwm_power_duty((int32_t)(500 - 1 * out_pos + 0 * out_gyro),
+        in_gyro = imu660ra_gyro_z; // 陀螺仪输入
-                              (int32_t)(500 + 1 * out_pos - 0 * out_gyro),
+        // in_angle = 0;                                                     // 图像远端输入
-                              (int32_t)(500 + out_speed + out_gyro),
+        // in_pos   = 0;                                                     // 图像近端输入
-                              (int32_t)(500 + out_speed - out_gyro));
+        //  清除计数
+        PID_Compute(&far_gyro_pid);
+
+        if (cnt1 >= 10) {
+            in_speed = -1 * sport_get_speed();
+            PID_Compute(&speed_pid);
+
+            PID_Compute(&near_pos_pid);
+
+            cnt1 = 0;
         }
 
+        if (cnt2 >= 20) {
+            cnt2 = 0;
+            PID_Compute(&far_angle_pid);
+        }
+        uint32_t pwm_duty_ls = (uint32_t)myclip_f(-1 * out_pos + 1.5 * out_gyro, 0.0f, 5000.f);
+        uint32_t pwm_duty_rs = (uint32_t)myclip_f(1 * out_pos - 1.5 * out_gyro, 0.0f, 5000.f);
+        uint32_t pwm_duty_lb = (uint32_t)myclip_f(1 * out_speed + out_gyro, 0.0f, 5000.f);
+        uint32_t pwm_duty_rb = (uint32_t)myclip_f(1 * out_speed - out_gyro, 0.0f, 5000.f);
+        by_pwm_power_duty(pwm_duty_ls, pwm_duty_rs, pwm_duty_lb, pwm_duty_rb);
+
         // by_pwm_power_duty(500+out_gyro, 500-out_gyro, 500+out_gyro , 500-out_gyro);
     } else {
-        by_pwm_power_duty(500, 500, 500, 500);
+        by_pwm_power_duty(0, 0, 0, 0);
     }
 
     /* 升力风扇设置 */
     if (bt_fly_flag == 0) {
         by_pwm_update_duty(0 + 500, 0 + 500);
     } else {
-        by_pwm_update_duty(bt_fly + 500, bt_fly + 500);
+        if (in_angle > 7.5 || in_angle < -7.5) {
+            by_pwm_update_duty(bt_fly + 450, bt_fly + 450);
+        } else {
+            by_pwm_update_duty(bt_fly + 500, bt_fly + 500);
+        }
     }
 }
 
@@ -137,7 +139,7 @@ void sport_pid_init()
     PID(&near_pos_pid, &in_pos, &out_pos, &set_pos, po_Kp, po_Ki, po_Kd, _PID_P_ON_E, _PID_CD_DIRECT);
     PID_SetMode(&near_pos_pid, _PID_MODE_AUTOMATIC);
     PID_SetSampleTime(&near_pos_pid, 10);
-    PID_SetOutputLimits(&near_pos_pid, -3000.0f, 3000.0f);
+    PID_SetOutputLimits(&near_pos_pid, -5000.0f, 5000.0f);
     // PID_Init(&near_pos_pid);
 
     /* 速度控制 */

app/main.c

@@ -62,13 +62,14 @@ int main(void)
 
     pit_ms_init(TIM1_PIT, 1); // 运动解算，编码器
 
-    // printf("ok\r\n");
+    printf("ok\r\n");
     by_vofa_init(&vofa_test, BT_UART_INDEX, sizeof(debug_array) / sizeof(debug_array[0]), debug_array);
 
     while (1) {
         debug_array[0] = in_pos;
         debug_array[1] = out_pos;
         // by_vofa_send(&vofa_test); // 发送数据
+        printf("pwm:%lu,%lu,%lu,%lu\r\n", pwm_duty_ls_g, pwm_duty_rs_g, pwm_duty_lb_g, pwm_duty_rb_g);
         Page_Run();
         by_frame_parse(2, &test_data[0].u32);
         by_buzzer_run();