底盘ok

User/module/chassis.cpp

@@ -1,5 +1,13 @@
 #include "module/chassis.hpp"
+#include "device/motor_rm.h"
+#include "task/user_task.h"
 #include "module/dr16.h"
+#include "bsp/time.h"
+
+#define CHASSIS_GAIN 100.0f
+
+#define LIMIT(x, min, max) \
+    do { if ((x) < (min)) (x) = (min); else if ((x) > (max)) (x) = (max); } while (0)
 
 Chassis::Chassis() {
     motors_param[0] = {
@@ -30,22 +38,78 @@ Chassis::Chassis() {
         .reverse = false,
         .gear = true
     };
+    pid_params[0] = KPID_Params_t{
+        .k=0.1,
+        .p=0.1,
+        .i=0,
+        .d=0,
+        .i_limit=0.5,
+        .out_limit=1,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
+    pid_params[1] = KPID_Params_t{
+        .k=0.1,
+        .p=0.1,
+        .i=0,
+        .d=0,
+        .i_limit=0.5,
+        .out_limit=1,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
+    pid_params[2] = KPID_Params_t{
+        .k=0.1,
+        .p=0.1,
+        .i=0,
+        .d=0,
+        .i_limit=0.5,
+        .out_limit=1,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
+    pid_params[3] = KPID_Params_t{
+        .k=0.1,
+        .p=0.1,
+        .i=0,
+        .d=0,
+        .i_limit=0.5,
+        .out_limit=1,
+        .d_cutoff_freq=100,
+        .range = -1,
+    };
 }
 
 void Chassis::init() {
-    MOTOR_RM_Register(&motors_param[0]);
+    for(int i = 0; i < 4; i++){
-    MOTOR_RM_Register(&motors_param[1]);
+        PID_Init(&pid[i], KPID_MODE_NO_D, CTRL_CHASSIS_FREQ, &pid_params[i]);
-    MOTOR_RM_Register(&motors_param[2]);
+        MOTOR_RM_Register(&motors_param[i]);
-    MOTOR_RM_Register(&motors_param[3]);
+    }
+    last_time = BSP_TIME_Get_us();
 }
 
 void Chassis::operator()() {
+    last_time = time;
+    time = BSP_TIME_Get_us();
+    dt = time - last_time;
     //更新数据
     update_motors();
     //解算底盘数据
-
+    calc_chassis();
     //计算pid
+    calc_pid();
+    if(dr16.data.sw_l == DR16_SW_DOWN){
+        MOTOR_RM_Relax(&motors_param[0]);
+        MOTOR_RM_Relax(&motors_param[1]);
+        MOTOR_RM_Relax(&motors_param[2]);
+        MOTOR_RM_Relax(&motors_param[3]);
+    }else {
+        for(int i = 0; i < 4; i++){
+            MOTOR_RM_SetOutput(&motors_param[i], pid_output[i]);
+        }
+    }
     //发送控制
+    send_control();
     // MOTOR_RM_SetOutput(&motors_param[0], 0.1f);
     // MOTOR_RM_Ctrl(&motors_param[0]);
 
@@ -68,14 +132,32 @@ void Chassis::update_motors(){
 
 void Chassis::calc_chassis(){
     if(dr16.data.sw_r == DR16_SW_DOWN){
+        target_speed.m1 = (dr16.data.ch_l_x - dr16.data.ch_l_y + dr16.data.ch_r_x) * CHASSIS_GAIN;  // 右前
+        target_speed.m2 = (-dr16.data.ch_l_x - dr16.data.ch_l_y + dr16.data.ch_r_x) * CHASSIS_GAIN;  // 右后
+        target_speed.m3 = (-dr16.data.ch_l_x + dr16.data.ch_l_y + dr16.data.ch_r_x) * CHASSIS_GAIN;  // 左后
+        target_speed.m4 = (dr16.data.ch_l_x + dr16.data.ch_l_y + dr16.data.ch_r_x) * CHASSIS_GAIN;  // 左前
 
+        /* 限幅，防止溢出 */
+        LIMIT(target_speed.m1, -1000, 1000);
+        LIMIT(target_speed.m2, -1000, 1000);
+        LIMIT(target_speed.m3, -1000, 1000);
+        LIMIT(target_speed.m4, -1000, 1000);
+    }else {
+        target_speed.m1 = 0;
+        target_speed.m2 = 0;
+        target_speed.m3 = 0;
+        target_speed.m4 = 0;
     }
 }
 
 void Chassis::calc_pid(){
+    pid_output[0] = PID_Calc(&pid[0], target_speed.m1, chassis_motors[0].feedback.rotor_speed, 0, dt/1000000.0f);
+    pid_output[1] = PID_Calc(&pid[1], target_speed.m2, chassis_motors[1].feedback.rotor_speed, 0, dt/1000000.0f);
+    pid_output[2] = PID_Calc(&pid[2], target_speed.m3, chassis_motors[2].feedback.rotor_speed, 0, dt/1000000.0f);
+    pid_output[3] = PID_Calc(&pid[3], target_speed.m4, chassis_motors[3].feedback.rotor_speed, 0, dt/1000000.0f);
 
 }
 
 void Chassis::send_control(){
-
+    MOTOR_RM_Ctrl(&motors_param[0]);
 }

User/module/chassis.hpp

@@ -4,6 +4,15 @@
 #include "bsp/can.h"
 #include "device/motor.h"
 #include "device/motor_rm.h"
+#include "component/pid.h"
+
+typedef struct
+{
+    float m1;
+    float m2;
+    float m3;
+    float m4;
+} motor_speed_t;
 
 class Chassis {
 public:
@@ -15,7 +24,14 @@ private:
     void calc_chassis();
     void calc_pid();
     void send_control();
+    KPID_t pid[4];
+    KPID_Params_t pid_params[4];
+    motor_speed_t target_speed = {0};
     MOTOR_RM_Param_t motors_param[4];
     MOTOR_RM_t chassis_motors[4];
+    float pid_output[4] = {0};
+    uint64_t time = 0;
+    uint64_t last_time = 0;
+    uint64_t dt = 0;
 };