ryan/rm_chassis
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

前馈

Browse Source
This commit is contained in:
zxy 2025-11-18 15:09:26 +08:00
parent 08f5a8043a
commit fe91017d69
2 changed files with 114 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
User/module/chassis.c
View File

@ -2,9 +2,6 @@
底盘模组 底盘模组
*/ */
/*
µ×ÅÌÄ£×é
*/
#include "cmsis_os2.h" #include "cmsis_os2.h"
#include <stdlib.h> #include <stdlib.h>
@ -81,6 +78,7 @@ int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
BSP_CAN_Init(); BSP_CAN_Init();
c->param = param; c->param = param;
c->mode = CHASSIS_MODE_RELAX; c->mode = CHASSIS_MODE_RELAX;
c->param->feedforward.coefficient = 0.1f; //前馈系数
//根据底盘不同设置模式轮子与混合器 //根据底盘不同设置模式轮子与混合器
Mixer_Mode_t mixer_mode; Mixer_Mode_t mixer_mode;
switch (param->type) { switch (param->type) {
@ -165,6 +163,7 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
return CHASSIS_OK; return CHASSIS_OK;
} }
/** /**
* @brief 底盘电机控制 * @brief 底盘电机控制
* @param c 底盘结构体指针 * @param c 底盘结构体指针
@ -190,15 +189,15 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, uint32_t now) {
c->move_vec.vx = c->move_vec.vy = 0.0f; c->move_vec.vx = c->move_vec.vy = 0.0f;
break; break;
case CHASSIS_MODE_INDEPENDENT: case CHASSIS_MODE_INDEPENDENT:
c->move_vec.vx = c_cmd->ctrl_vec.vx; c->move_vec.vx = c_cmd->ctrl_vec.vx ;
c->move_vec.vy = c_cmd->ctrl_vec.vy; c->move_vec.vy = c_cmd->ctrl_vec.vy ;
break; break;
default: { //遥控器坐标->机体坐标系 default: { //遥控器坐标->机体坐标系
float beta = c->feedback.encoder_gimbalYawMotor - c->mech_zero; float beta = c->feedback.encoder_gimbalYawMotor - c->mech_zero;
float cosb = cosf(beta); float cosb = cosf(beta);
float sinb = sinf(beta); float sinb = sinf(beta);
c->move_vec.vx = cosb * c_cmd->ctrl_vec.vx - sinb * c_cmd->ctrl_vec.vy; c->move_vec.vx = cosb * c_cmd->ctrl_vec.vx - sinb * c_cmd->ctrl_vec.vy ;
c->move_vec.vy = sinb * c_cmd->ctrl_vec.vx + cosb * c_cmd->ctrl_vec.vy; c->move_vec.vy = sinb * c_cmd->ctrl_vec.vx + cosb * c_cmd->ctrl_vec.vy ;
break; break;
} }
} }
@ -210,19 +209,19 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, uint32_t now) {
c->move_vec.wz = 0.0f; c->move_vec.wz = 0.0f;
break; break;
case CHASSIS_MODE_OPEN: case CHASSIS_MODE_OPEN:
c->move_vec.wz = c_cmd->ctrl_vec.wz; c->move_vec.wz = c_cmd->ctrl_vec.wz ;
break; break;
//云台跟随 //云台跟随
case CHASSIS_MODE_FOLLOW_GIMBAL: case CHASSIS_MODE_FOLLOW_GIMBAL:
c->move_vec.wz = PID_Calc(&c->pid.follow, c->mech_zero, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt); c->move_vec.wz = PID_Calc(&c->pid.follow, c->mech_zero, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt) ;
break; break;
//云台跟随（偏移） //云台跟随（偏移）
case CHASSIS_MODE_FOLLOW_GIMBAL_35: case CHASSIS_MODE_FOLLOW_GIMBAL_35:
c->move_vec.wz = PID_Calc(&c->pid.follow,c->mech_zero +M_7OVER72PI, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt); c->move_vec.wz = PID_Calc(&c->pid.follow,c->mech_zero +M_7OVER72PI, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt) ;
break; break;
//小陀螺 //小陀螺
case CHASSIS_MODE_ROTOR: case CHASSIS_MODE_ROTOR:
c->move_vec.wz = c->wz_multi * Chassis_CalcWz(CHASSIS_ROTOR_WZ_MIN,CHASSIS_ROTOR_WZ_MAX, now); c->move_vec.wz = c->wz_multi * Chassis_CalcWz(CHASSIS_ROTOR_WZ_MIN,CHASSIS_ROTOR_WZ_MAX, now) ;
break; break;
} }
//运动学逆解算，运动向量分解为电机转速 //运动学逆解算，运动向量分解为电机转速
@ -240,12 +239,15 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, uint32_t now) {
case CHASSIS_MODE_ROTOR: case CHASSIS_MODE_ROTOR:
case CHASSIS_MODE_INDEPENDENT: case CHASSIS_MODE_INDEPENDENT:
out_current = PID_Calc(&c->pid.motor[i], c->setpoint.motor_rpm[i], fb, 0.0f, c->dt); out_current = PID_Calc(&c->pid.motor[i], c->setpoint.motor_rpm[i], fb, 0.0f, c->dt);
const float K_FF = c->param->feedforward.coefficient; //前馈系数，具体值在初始化部分，使用时可以自己修改。
float feedforward_current = K_FF * c->setpoint.motor_rpm[i];
out_current += feedforward_current; //总电流 = 前馈电流 + 反馈电流
break; break;
case CHASSIS_MODE_OPEN: case CHASSIS_MODE_OPEN:
out_current = c->setpoint.motor_rpm[i] / 7000.0f; out_current = c->setpoint.motor_rpm[i] / 7000.0f ;
break; break;
case CHASSIS_MODE_RELAX: case CHASSIS_MODE_RELAX:
out_current = 0.0f; out_current = 0.0f ;
break; break;
} }

193
User/module/chassis.h
View File

@ -1,5 +1,5 @@
/* /*
底盘模组 底盘头文件
*/ */
#pragma once #pragma once
@ -17,72 +17,72 @@ extern "C" {
#include "component\ahrs.h" #include "component\ahrs.h"
#include "device/motor_rm.h" #include "device/motor_rm.h"
/* Exported constants ------------------------------------------------------- */ /* Exported constants ------------------------------------------------------- */
#define CHASSIS_OK (0) /* 运行正常 */ #define CHASSIS_OK (0) /*运行正常*/
#define CHASSIS_ERR (-1) /* 运行时出现了一些小错误 */ #define CHASSIS_ERR (-1) /*运行错误*/
#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */ #define CHASSIS_ERR_NULL (-2) /*空指针错误*/
#define CHASSIS_ERR_MODE (-3) /* 运行时出配置了错误的ChassisMode_t */ #define CHASSIS_ERR_MODE (-3) /*模式错误*/
#define CHASSIS_ERR_TYPE (-4) /* 运行时出配置了错误的Chassis_Type_t */ #define CHASSIS_ERR_TYPE (-4) /*类型错误*/
#define MAX_MOTOR_CURRENT 20.0f #define MAX_MOTOR_CURRENT 20.0f
/* 底盘控制模式 */ /* 底盘控制模式 */
typedef enum { typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式,电机不输出。一般情况底盘初始化之后的模式 */ CHASSIS_MODE_RELAX, /*放松模式，电机不转动*/
CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制静止。用于机器人停止状态 */ CHASSIS_MODE_BREAK, /* 制动模式，电机立即停止 */
CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */ CHASSIS_MODE_FOLLOW_GIMBAL, /* 跟随模式，电机根据云台角度旋转 */
CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 通过闭环控制使车头方向35°跟随云台 */ CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 跟随模式，电机根据云台角度旋转35度 */
CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */ CHASSIS_MODE_ROTOR, /* 旋转模式，电机根据旋转角度旋转 */
CHASSIS_MODE_INDEPENDENT, /*独立模式。底盘运行不受云台影响 */ CHASSIS_MODE_INDEPENDENT,/* 独立模式，电机根据指令独立旋转 */
CHASSIS_MODE_OPEN, /* 开环模式。底盘运行不受PID控制，直接输出到电机 */ CHASSIS_MODE_OPEN, /* 开放模式，电机根据指令直接旋转 */
} Chassis_Mode_t; } Chassis_Mode_t;
/* 小陀螺转动模式 */ /* 底盘小陀螺模式 */
typedef enum { typedef enum {
ROTOR_MODE_CW, /* 顺时针转动 */ ROTOR_MODE_CW, // 顺时针旋转
ROTOR_MODE_CCW, /* 逆时针转动 */ ROTOR_MODE_CCW, // 逆时针旋转
ROTOR_MODE_RAND, /* 随机转动 */ ROTOR_MODE_RAND, // 随机旋转
} Chassis_RotorMode_t; } Chassis_RotorMode_t;
/* 底盘控制命令 */ /* 底盘控制命令 */
typedef struct { typedef struct {
Chassis_Mode_t mode; /* 底盘运行模式 */ Chassis_Mode_t mode; /* 底盘运动模式*/
Chassis_RotorMode_t mode_rotor; /* 小陀螺转动模式 */ Chassis_RotorMode_t mode_rotor; /* 小陀螺模式旋转方向*/
MoveVector_t ctrl_vec; /* 底盘控制向量 */ MoveVector_t ctrl_vec; /* 底盘运动控制向量*/
} Chassis_CMD_t; } Chassis_CMD_t;
/* 限位 */ /* 限位好像没用到 */
typedef struct { typedef struct {
float max; float max;
float min; float min;
} Chassis_Limit_t; } Chassis_Limit_t;
/* 底盘类型（底盘的物理设计） */ /* 底盘类型 */
typedef enum { typedef enum {
CHASSIS_TYPE_MECANUM, /* 麦克纳姆轮 */ CHASSIS_TYPE_MECANUM, /* 麦克纳姆轮底盘 */
CHASSIS_TYPE_PARLFIX4, /* 平行摆放的四个驱动轮 */ CHASSIS_TYPE_PARLFIX4, /* 平行固定4轮底盘 */
CHASSIS_TYPE_PARLFIX2, /* 平行摆放的两个驱动轮 */ CHASSIS_TYPE_PARLFIX2, /* 平行固定2轮底盘 */
CHASSIS_TYPE_OMNI_CROSS, /* 叉型摆放的四个全向轮 */ CHASSIS_TYPE_OMNI_CROSS, /* 十字.omni轮底盘 */
CHASSIS_TYPE_OMNI_PLUS, /* 十字型摆设的四个全向轮 */ CHASSIS_TYPE_OMNI_PLUS, /* X型.omni轮底盘 */
CHASSIS_TYPE_DRONE, /* 无人机底盘 */ CHASSIS_TYPE_DRONE, /* 无人机底盘 */
CHASSIS_TYPE_SINGLE, /* 单个摩擦轮 */ CHASSIS_TYPE_SINGLE, /* 单轮底盘 */
} Chassis_Type_t; } Chassis_Type_t;
/* 底盘参数结构体,ALL初始化参数 */ /* 底盘参数结构体 ，初始化参数*/
typedef struct { typedef struct {
MOTOR_RM_Param_t motor_param[4]; MOTOR_RM_Param_t motor_param[4];
struct { struct {
KPID_Params_t motor_pid_param; /* 底盘电机PID参数 */ KPID_Params_t motor_pid_param; /* 电机转速PID参数 */
KPID_Params_t follow_pid_param; /* 跟随云台PID参数 */ KPID_Params_t follow_pid_param; /* 跟随PID参数 */
} pid; } pid;
Chassis_Type_t type; /* 底盘类型，底盘的机械设计和轮子选型 */ Chassis_Type_t type; /* 底盘类型 */
/* 低通滤波器截至频率*/ /* 低通滤波器截止频率设置 */
struct { struct {
float in; /* 输入 */ float in; /* 输入 */
float out; /* 输出 */ float out; /* 输出 */
} low_pass_cutoff_freq; } low_pass_cutoff_freq;
/* 电机反装，应该和云台设置相同*/ /*电机反装，设置与云台相同，但貌似没啥用*/
struct { struct {
bool yaw; bool yaw;
} reverse; } reverse;
@ -90,6 +90,12 @@ typedef struct {
float max_vx, max_vy, max_wz; float max_vx, max_vy, max_wz;
float max_current; float max_current;
} limit; } limit;
/*前馈相关系数*/
struct {
float coefficient;
} feedforward;
} Chassis_Params_t; } Chassis_Params_t;
typedef struct { typedef struct {
@ -98,59 +104,56 @@ typedef struct {
} Chassis_IMU_t; } Chassis_IMU_t;
typedef struct { typedef struct {
MOTOR_Feedback_t motor[4]; // 四个 3508电机 反馈 MOTOR_Feedback_t motor[4]; /* 电机反馈 */
float encoder_gimbalYawMotor; float encoder_gimbalYawMotor;
} Chassis_Feedback_t; } Chassis_Feedback_t;
/* 底盘输出结构体*/ /* 底盘输出结构体 */
typedef struct { typedef struct {
float motor[4]; float motor[4]; /* 电机输出电流目标值，范围[-1,1],对应最大电流 */
} Chassis_Output_t; } Chassis_Output_t;
/* /* 底盘主结构体 */
* 运行的主结构体￣
* 包含初始化参数,中间变量,输出变量
*/
typedef struct { typedef struct {
uint64_t last_wakeup; uint64_t last_wakeup;
float dt; float dt;
Chassis_Params_t *param; /* 底盘参数,用Chassis_Init设定 */ Chassis_Params_t *param; /* 底盘参数指针 */
/* 模块通用 */ /* 底盘状态 */
Chassis_Mode_t mode; /* 底盘模式 */ Chassis_Mode_t mode; /* 底盘运动模式 */
/* 底盘设计 */ /* 底盘设计 */
int8_t num_wheel; /* 底盘轮子数量 */ int8_t num_wheel; /* 底盘轮子数量 */
Mixer_t mixer; /* 混合器,移动向量->电机目标值 */ Mixer_t mixer; /* 混合器 */
MoveVector_t move_vec; /* 底盘实际的运动向量 */ MoveVector_t move_vec; /* 底盘运动控制向量 */
MOTOR_RM_t *motors[4];/*指向底盘每个电机参数*/ MOTOR_RM_t *motors[4]; /* 电机指针数组 */
float mech_zero; float mech_zero; /* 机械零点 */
float wz_multi; /* 小陀螺旋转模式 */ float wz_multi; /* 小陀螺模式旋转角度系数 */
/* PID计算目标值 */ /*PID计算目标值*/
struct { struct {
float motor_rpm[4]; /* 电机转速的动态数组,单位:RPM */ float motor_rpm[4]; /* 电机转速目标值，单位rpm */
} setpoint; } setpoint;
/* 反馈控制用的PID */ /* 反馈控制PID计算 */
struct { struct {
KPID_t motor[4]; /* 控制轮子电机用的PID的动态数组 */ KPID_t motor[4]; /* 电机转速PID */
KPID_t follow; /* 跟随云台用的PID */ KPID_t follow; /* 跟随PID */
} pid; } pid;
struct { struct {
Chassis_Limit_t vx, vy, wz; Chassis_Limit_t vx, vy, wz;
} limit; } limit;
/* 滤波器 */ /*滤波器*/
struct { struct {
LowPassFilter2p_t in[4]; /* 反馈值滤波器 */ LowPassFilter2p_t in[4]; /* 电机速度反馈滤波器 */
LowPassFilter2p_t out[4]; /* 输出值滤波器 */ LowPassFilter2p_t out[4]; /* 电机速度输出滤波器 */
} filter; } filter;
Chassis_Output_t out; /* 电机输出 */ Chassis_Output_t out;; /* 底盘输出结构体 */
Chassis_Feedback_t feedback; Chassis_Feedback_t feedback;
//float out_motor[4]; //float out_motor[4];
} Chassis_t; } Chassis_t;
@ -158,73 +161,73 @@ typedef struct {
/* Exported functions prototypes -------------------------------------------- */ /* Exported functions prototypes -------------------------------------------- */
/** /**
* \brief 底盘初始化 * \brief 底盘初始化
* *
* \param c 包含底盘数据的结构体 * \param c 底盘结构体指针
* \param param 包含底盘参数的结构体指针 * \param param 底盘参数指针
* \param target_freq 任务预期的运行频率 * \param target_freq 目标频率，单位Hz
* *
* \return 运行结果 * \return 运行结果，CHASSIS_OK:成功
*/ */
int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param, int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
float target_freq); float target_freq);
/** /**
* \brief 更新底盘反馈信息 * \brief 更新底盘反馈
* *
* \param c 包含底盘数据的结构体 * \param c 底盘结构体指针
* \param can CAN设备结构体
* *
* \return 运行结果 * \return 运行结果，CHASSIS_OK:成功
*/ */
int8_t Chassis_UpdateFeedback(Chassis_t *c); int8_t Chassis_UpdateFeedback(Chassis_t *c);
/** /**
* \brief 运行底盘控制逻辑 * \brief 底盘控制
* *
* \param c 包含底盘数据的结构体 * \param c 底盘结构体指针
* \param c_cmd 底盘控制指令 * \param c_cmd 底盘控制命令指针
* \param dt_sec 两次调用的时间间隔 * \param now 当前时间戳，单位ms
* *
* \return 运行结果 * \return 运行结果，CHASSIS_OK:成功
*/ */
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd,
uint32_t now); uint32_t now);
/** /**
* \brief 复制底盘输出值 * \brief 底盘输出
* * \param c 底盘结构体指针
* \param s 包含底盘数据的结构体
* \param out CAN设备底盘输出结构体
*/ */
void Chassis_Output(Chassis_t *c); void Chassis_Output(Chassis_t *c);
/** /**
* \brief 清空Chassis输出数据 * \brief 底盘输出重置
* *
* \param out CAN设备底盘输出结构体 * \param c 底盘结构体指针
*/ */
void Chassis_ResetOutput(Chassis_t *c); void Chassis_ResetOutput(Chassis_t *c);
/** /**
* @brief 底盘功率限制 * \brief 底盘功率限制
* *
* @param c 底盘数据 * \param c 底盘结构体指针
* @param cap 电容数据 * \param cap 电容数据指针
* @param ref 裁判系统数据 * \param ref 比赛数据指针
* @return 函数运行结果 *
* \return 运行结果，CHASSIS_OK:成功
*/ */
//还没有加入，waiting。。。。。。int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap, //<EFBFBD><EFBFBD>û<EFBFBD>м<EFBFBD><EFBFBD>룬waiting<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap,
// const Referee_ForChassis_t *ref); // const Referee_ForChassis_t *ref);
/** /**
* @brief 导出底盘数据 * \brief 底盘UI数据填充
* *
* @param chassis 底盘数据结构体 * \param c 底盘结构体指针
* @param ui UI数据结构体 * \param ui 底盘UI数据指针
*
* \return 无
*/ */
//void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui); //void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);