From fe91017d69b8d62f7476884505ba3e7489b6d87c Mon Sep 17 00:00:00 2001 From: zxy Date: Tue, 18 Nov 2025 15:09:26 +0800 Subject: [PATCH] =?UTF-8?q?=E5=89=8D=E9=A6=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/chassis.c | 28 +++--- User/module/chassis.h | 195 +++++++++++++++++++++--------------------- 2 files changed, 114 insertions(+), 109 deletions(-) diff --git a/User/module/chassis.c b/User/module/chassis.c index 082d908..a3bbe49 100644 --- a/User/module/chassis.c +++ b/User/module/chassis.c @@ -2,9 +2,6 @@ 底盘模组 */ -/* -底盘模组 -*/ #include "cmsis_os2.h" #include @@ -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; } diff --git a/User/module/chassis.h b/User/module/chassis.h index 233d85c..bc387e7 100644 --- a/User/module/chassis.h +++ b/User/module/chassis.h @@ -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; /* 鐢垫満杞烶ID鍙傛暟 */ + 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]; /* 鐢垫満杞烶ID */ + 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 鐩爣棰戠巼锛屽崟浣岺z * - * \return 运行结果 + * \return 杩愯缁撴灉锛孋HASSIS_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 杩愯缁撴灉锛孋HASSIS_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 杩愯缁撴灉锛孋HASSIS_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 杩愯缁撴灉锛孋HASSIS_OK:鎴愬姛 */ -//还没有加入，waiting。。。。。。int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap, +//锟斤拷没锟叫硷拷锟诫，waiting锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷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);