From 02ca6c8130c117dce3223b0047434fad8161e6fb Mon Sep 17 00:00:00 2001 From: Feelyx Date: Wed, 14 Jan 2026 17:33:05 +0800 Subject: [PATCH] =?UTF-8?q?=E5=BA=95=E7=9B=98ok?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/chassis.cpp | 96 ++++++++++++++++++++++++++++++++++++++--- User/module/chassis.hpp | 16 +++++++ 2 files changed, 105 insertions(+), 7 deletions(-) diff --git a/User/module/chassis.cpp b/User/module/chassis.cpp index 04d38a4..0dbc1f0 100644 --- a/User/module/chassis.cpp +++ b/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]); - MOTOR_RM_Register(&motors_param[1]); - MOTOR_RM_Register(&motors_param[2]); - MOTOR_RM_Register(&motors_param[3]); + for(int i = 0; i < 4; i++){ + PID_Init(&pid[i], KPID_MODE_NO_D, CTRL_CHASSIS_FREQ, &pid_params[i]); + MOTOR_RM_Register(&motors_param[i]); + } + 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]); } diff --git a/User/module/chassis.hpp b/User/module/chassis.hpp index 400ed99..90554b5 100644 --- a/User/module/chassis.hpp +++ b/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; };