Feelyx/mv_arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

底盘ok

Browse Source
This commit is contained in:
Feelyx 2026-01-14 17:33:05 +08:00
parent 554d930467
commit 02ca6c8130
2 changed files with 105 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
User/module/chassis.cpp
View File

@ -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]);
}

16
User/module/chassis.hpp
View File

@ -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;
};