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 <stdlib.h>
@ -81,6 +78,7 @@ int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
BSP_CAN_Init();
c->param = param;
c->mode = CHASSIS_MODE_RELAX;
c->param->feedforward.coefficient = 0.1f; //前馈系数
//根据底盘不同设置模式轮子与混合器
Mixer_Mode_t mixer_mode;
switch (param->type) {
@ -165,6 +163,7 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
return CHASSIS_OK;
}
/**
* @brief 底盘电机控制
* @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;
break;
case CHASSIS_MODE_INDEPENDENT:
c->move_vec.vx = c_cmd->ctrl_vec.vx;
c->move_vec.vy = c_cmd->ctrl_vec.vy;
c->move_vec.vx = c_cmd->ctrl_vec.vx ;
c->move_vec.vy = c_cmd->ctrl_vec.vy ;
break;
default: { //遥控器坐标->机体坐标系
float beta = c->feedback.encoder_gimbalYawMotor - c->mech_zero;
float cosb = cosf(beta);
float sinb = sinf(beta);
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.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 ;
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;
break;
case CHASSIS_MODE_OPEN:
c->move_vec.wz = c_cmd->ctrl_vec.wz;
c->move_vec.wz = c_cmd->ctrl_vec.wz ;
break;
//云台跟随
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;
//云台跟随（偏移）
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;
//小陀螺
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;
}
//运动学逆解算，运动向量分解为电机转速
@ -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_INDEPENDENT:
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;
case CHASSIS_MODE_OPEN:
out_current = c->setpoint.motor_rpm[i] / 7000.0f;
out_current = c->setpoint.motor_rpm[i] / 7000.0f ;
break;
case CHASSIS_MODE_RELAX:
out_current = 0.0f;
out_current = 0.0f ;
break;
}

195
User/module/chassis.h
View File

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