init:全链路闭环控制调通

2026-06-06 21:26:08 +08:00
parent b5be64b642
commit f28d129e11
32 changed files with 5120 additions and 181 deletions
@@ -0,0 +1,158 @@
+#include "Task.h"
+#include "zf_driver_pit.h"
+#include "zf_device_key.h"
+#include "zf_common_interrupt.h"
+#include "IMU.h"
+#include "Beep.h"
+#include "GrayscaleSensor.h"
+#include "Motor.h"
+#include "MotorEncoder.h"
+#include "PID_Controller.h"
+#include "Command.h"
+
+
+volatile uint8_t motor_control_task_enable = 0;
+volatile uint8_t angle_control_task_enable = 0;
+volatile uint8_t position_control_task_enable = 0;
+
+/*速度环的输入，开启角度环后由角度环接管(范围-50到+50)*/
+volatile int16_t motor_speed_right_ref = 0;
+volatile int16_t motor_speed_left_ref = 0;
+/*角度环接管速度之后的基础直行速度*/
+volatile int16_t motor_speed_base = 0;
+/*角度环输入，开启位置环后由位置环接管*/
+volatile float yaw_ref = 0;
+/*位置环的输出*/
+volatile float angle_velocity = 0;
+
+volatile PIDInstance pid_speed_right={0};
+volatile PIDInstance pid_speed_left={0};
+volatile PIDInstance pid_yaw = {0};
+volatile PIDInstance pid_position = {0};
+
+
+void motor_control_init(float T);
+void angle_control_init(float T);
+void position_control_init(float T);
+void angle_control_task(void);
+void position_control_task(void);
+void motor_control_task(void);
+void Task_10ms(uint32 state, void *ptr)
+{
+    // 10ms 执行一次的任务代码(100Hz)
+    motor_encoder_update();
+    if(motor_control_task_enable)
+        motor_control_task();
+    
+}
+void Task_20ms(uint32 state, void *ptr)
+{
+    // 20ms 执行一次的任务代码(50Hz)
+    grayscale_sensor_scan();
+    if(position_control_task_enable)
+        position_control_task();
+
+    imu_update();
+    if(angle_control_task_enable)
+        angle_control_task();
+}
+void Task_50ms(uint32 state, void *ptr)
+{
+    // 50ms 执行一次的任务代码(20Hz)
+    key_scanner();
+    beep_actuator();
+}
+void task_init(void)
+{
+    motor_control_init(0.01f);
+    angle_control_init(0.02f);
+    position_control_init(0.02f);
+
+    pit_ms_init(PIT_TIM_A0,10,Task_10ms,NULL);
+    interrupt_set_priority(TIMA0_INT_IRQn,1);
+    pit_ms_init(PIT_TIM_A1,20,Task_20ms,NULL);
+    interrupt_set_priority(TIMA1_INT_IRQn,1);
+    pit_ms_init(PIT_TIM_G6,50,Task_50ms,NULL);
+    interrupt_set_priority(TIMG6_INT_IRQn,7);
+}
+
+void motor_control_init(float T)
+{
+    PID_Init_Config_s config_right = {
+        .Kp = 300.0f,
+        .Ki = 150.0f,
+        .Kd = 0.0f,
+        .T = T,
+        .MaxOut = MOTOR_MAX
+    };
+    PIDInit((PIDInstance *)&pid_speed_right, &config_right);
+    PID_Init_Config_s config_left = {
+        .Kp = 300.0f,
+        .Ki = 150.0f,
+        .Kd = 0.0f,
+        .T = T,
+        .MaxOut = MOTOR_MAX
+    };
+    PIDInit((PIDInstance *)&pid_speed_left, &config_left);
+}
+void motor_control_task(void)
+{
+    float pwm_right = PIDCalculate((PIDInstance *)&pid_speed_right,motor_speed_right.value,motor_speed_right_ref);
+    float pwm_left = PIDCalculate((PIDInstance *)&pid_speed_left,motor_speed_left.value,motor_speed_left_ref);
+    
+    motor_set_pwm_right(pwm_right);
+    motor_set_pwm_left(pwm_left);
+}
+void angle_control_init(float T)
+{
+    PID_Init_Config_s config_yaw = {
+        .Kp = 1.0f,
+        .Ki = 0.0f,
+        .Kd = 0.0f,
+        .T = T,
+        .MaxOut = 25,
+        .Improve = PID_Yaw_Error_Corretion
+    };
+    PIDInit((PIDInstance *)&pid_yaw, &config_yaw);
+}
+void angle_control_task(void)
+{
+    float speed_dif = PIDCalculate((PIDInstance *)&pid_yaw,imu_angle.yaw,yaw_ref);
+    motor_speed_left_ref = motor_speed_base - speed_dif;
+    motor_speed_right_ref = motor_speed_base + speed_dif;
+}
+void position_control_init(float T)
+{
+    PID_Init_Config_s config_position = {
+        .Kp = 0.5f,
+        .Ki = 0.0f,
+        .Kd = 0.0f,
+        .T = T,
+        .MaxOut = 90,
+    };
+    PIDInit((PIDInstance *)&pid_position, &config_position);
+}
+void position_control_task(void)
+{
+    //角度速率（单位度/秒）
+    angle_velocity = PIDCalculate((PIDInstance *)&pid_position,grayscale_sensor_position,0);
+
+    //发送给云台，角度速率前馈
+    command_send_angle_velocity((float*)&angle_velocity);
+
+    set_yaw_ref(yaw_ref + angle_velocity*pid_position.dt);
+}
+void set_yaw_ref(float set_value)
+{
+    if(set_value>180)
+    {
+        while(set_value > 180)
+            set_value -= 360;
+    }
+    else if(set_value<-180)
+    {
+        while(set_value < -180)
+            set_value += 360;
+    }
+    yaw_ref = set_value;
+}