xm/god-yuan-hero
xm/god-yuan-hero
1
1
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

111

Browse Source
This commit is contained in:
yxming66 2025-12-28 14:20:06 +08:00
parent e1e8329d28
commit 407816a5a2
10 changed files with 124 additions and 620 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -42,6 +42,7 @@ target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE
# Add user defined library search paths
)
file(GLOB SINGLE_CHAR_FILES "User/module/cmd_v2/*.c")
# Add sources to executable
target_sources(${CMAKE_PROJECT_NAME} PRIVATE
# Add user sources here
@ -78,6 +79,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/module/gimbal.c
User/module/shoot.c
${SINGLE_CHAR_FILES}
User/module/track.c
# User/task sources
User/task/atti_esti.c
User/task/blink.c

2
User/module/chassis.c
View File

@ -283,8 +283,6 @@ void Chassis_Output(Chassis_t *c) {
MOTOR_RM_SetOutput(&rm->param, c->out.motor[i]);
}
//µ÷ÓÃctrl
for (uint8_t i = 0; i < c->num_wheel; i++) {
MOTOR_RM_t *rm = c->motors[i];

299
User/module/chassis_hahaha.c
View File

@ -1,299 +0,0 @@
// /*
// 底盘模组
// */
// /*
// 底盘模组
// */
// #include "cmsis_os2.h"
// #include <stdlib.h>
// #include "bsp/mm.h"
// #include "bsp/can.h"
// #include "component/ahrs.h"
// #include "device/motor_rm.h"
// #include "device/motor.h"
// #include "module/chassis.h"
// /**
// * @brief 底盘小陀螺模式相关参数
// */
// #define CHASSIS_ROTOR_WZ_MIN 0.6f //小陀螺最小速度
// #define CHASSIS_ROTOR_WZ_MAX 0.8f //小陀螺最大速度
// #define M_7OVER72PI (M_2PI * 7.0f / 72.0f) //角度偏移量（用在跟随云台35°）
// #define CHASSIS_ROTOR_OMEGA 0.001f //角速度变化频率
// /**
// * @brief 设置底盘模式
// * @param c 底盘结构体指针
// * @param mode 目标控制模式
// * @param now 当前时间戳(ms)
// * @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空
// */
// static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode, uint32_t now) {
// if (!c)
// return CHASSIS_ERR_NULL;
// if (mode == c->mode)
// return CHASSIS_OK;
// //随机种子，小陀螺模式随机设置旋转方向
// if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) {
// srand(now);
// c->wz_multi = (rand() % 2) ? -1 : 1;
// }
// //重置PID和滤波
// for (uint8_t i = 0; i < c->num_wheel; i++) {
// PID_Reset(&c->pid.motor[i]);
// LowPassFilter2p_Reset(&c->filter.in[i], 0.0f);
// LowPassFilter2p_Reset(&c->filter.out[i], 0.0f);
// }
// c->mode = mode;
// return CHASSIS_OK;
// }
// /**
// * @brief 小陀螺模式动态角速度
// * @param min 最小速度
// * @param max 最大速度
// * @param now 当前时间戳(ms)
// * @return 角速度值
// */
// static float Chassis_CalcWz(const float min, const float max, uint32_t now) {
// float wz_vary = fabsf(0.2f * sinf(CHASSIS_ROTOR_OMEGA * (float)now)) + min;
// return (wz_vary > max) ? max : wz_vary;
// }
// /**
// * @brief 底盘模式初始化
// * @param c 底盘结构体指针
// * @param param 底盘参数结构体指针
// * @param mech_zero 机械零点欧拉角
// * @param target_freq 控制频率(Hz)
// * @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空 CHASSIS_ERR_TYPE:不支持的模式
// */
// int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
// float target_freq) {
// if (!c) return CHASSIS_ERR_NULL;
// //初始化CAN通信
// BSP_CAN_Init();
// c->param = param;
// c->mode = CHASSIS_MODE_RELAX;
// //根据底盘不同设置模式轮子与混合器
// Mixer_Mode_t mixer_mode;
// switch (param->type) {
// case CHASSIS_TYPE_MECANUM://麦轮
// c->num_wheel = 4;
// mixer_mode = MIXER_MECANUM;
// break;
// case CHASSIS_TYPE_PARLFIX4:
// c->num_wheel = 4;
// mixer_mode = MIXER_PARLFIX4;
// break;
// case CHASSIS_TYPE_PARLFIX2:
// c->num_wheel = 2;
// mixer_mode = MIXER_PARLFIX2;
// break;
// case CHASSIS_TYPE_OMNI_CROSS:
// c->num_wheel = 4;
// mixer_mode = MIXER_OMNICROSS;
// break;
// case CHASSIS_TYPE_OMNI_PLUS: //全向轮（老步兵类型）
// c->num_wheel = 4;
// mixer_mode = MIXER_OMNIPLUS;
// break;
// case CHASSIS_TYPE_SINGLE:
// c->num_wheel = 1;
// mixer_mode = MIXER_SINGLE;
// break;
// default:
// return CHASSIS_ERR_TYPE;
// }
// //初始化时间戳
// c->last_wakeup = 0;
// c->dt = 0.0f;
// //初始化PID和滤波
// for (uint8_t i = 0; i < c->num_wheel; i++) {
// PID_Init(&c->pid.motor[i], KPID_MODE_NO_D, target_freq, &param->pid.motor_pid_param);
// LowPassFilter2p_Init(&c->filter.in[i], target_freq, param->low_pass_cutoff_freq.in);
// LowPassFilter2p_Init(&c->filter.out[i], target_freq, param->low_pass_cutoff_freq.out);
// //清零电机反馈
// c->feedback.motor[i].rotor_speed = 0;
// c->feedback.motor[i].torque_current = 0;
// c->feedback.motor[i].rotor_abs_angle = 0;
// c->feedback.motor[i].temp = 0;
// }
// //初始化PID和混合器
// PID_Init(&c->pid.follow, KPID_MODE_NO_D, target_freq, &param->pid.follow_pid_param);
// Mixer_Init(&c->mixer, mixer_mode);
// //清零运动向量和输出
// c->move_vec.vx = c->move_vec.vy = c->move_vec.wz = 0.0f;
// for (uint8_t i = 0; i < c->num_wheel; i++) {
// c->out.motor[i] = 0.0f;
// }
// //注册大疆电机
// for (int i = 0; i < c->num_wheel; i++) {
// MOTOR_RM_Register(&(c->param->motor_param[i]));
// }
// return CHASSIS_OK;
// }
// /**
// * @brief 更新电机反馈(IMU+电机状态)
// * @param c 底盘结构体指针
// * @param feedback 底盘反馈指针结构体
// * @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空
// */
// int8_t Chassis_UpdateFeedback(Chassis_t *c) {
// //更新所有电机反馈
// for (uint8_t i = 0; i < c->num_wheel; i++) {
// MOTOR_RM_Update(&(c->param->motor_param[i]));
// MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(c->param->motor_param[i]));
// c->motors[i] = rm_motor;
// MOTOR_RM_t *rm = c->motors[i];
// if (rm_motor != NULL) {
// c->feedback.motor[i] = rm_motor->feedback;
// }else
// {
// return CHASSIS_ERR_NULL;
// }
// }
// return CHASSIS_OK;
// }
// /**
// * @brief 底盘电机控制
// * @param c 底盘结构体指针
// * @param c_cmd 控制命令
// * @param now 当前时间戳(ms)
// * @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空
// */
// int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, uint32_t now) {
// if (!c || !c_cmd) return CHASSIS_ERR_NULL;
// //计算控制周期
// c->dt = (float)(now - c->last_wakeup) / 1000.0f;
// c->last_wakeup = now;
// if (!isfinite(c->dt) || c->dt <= 0.0f) {
// c->dt = 0.001f;
// }
// if (c->dt < 0.0005f) c->dt = 0.0005f;
// if (c->dt > 0.050f) c->dt = 0.050f;
// //设置模式
// Chassis_SetMode(c, c_cmd->mode, now);
// //不同模式下对应解算（运动向量）
// switch (c->mode) {
// case CHASSIS_MODE_BREAK:
// 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;
// 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;
// break;
// }
// }
// //根据模式计算底盘角速度
// switch (c->mode) {
// case CHASSIS_MODE_RELAX:
// case CHASSIS_MODE_BREAK:
// case CHASSIS_MODE_INDEPENDENT:
// c->move_vec.wz = 0.0f;
// break;
// case CHASSIS_MODE_OPEN:
// 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);
// 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);
// break;
// //小陀螺
// case CHASSIS_MODE_ROTOR:
// c->move_vec.wz = c->wz_multi * Chassis_CalcWz(CHASSIS_ROTOR_WZ_MIN,CHASSIS_ROTOR_WZ_MAX, now);
// break;
// }
// //运动学逆解算，运动向量分解为电机转速
// Mixer_Apply(&c->mixer, &c->move_vec, c->setpoint.motor_rpm, c->num_wheel, 500.0f);
// for (uint8_t i = 0; i < c->num_wheel; i++) {
// float rf = c->setpoint.motor_rpm[i];///目标转速
// float fb = LowPassFilter2p_Apply(&c->filter.in[i], (float)c->feedback.motor[i].rotor_speed);
// float out_current;
// switch (c->mode) {
// case CHASSIS_MODE_BREAK:
// case CHASSIS_MODE_FOLLOW_GIMBAL:
// case CHASSIS_MODE_FOLLOW_GIMBAL_35:
// 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);
// break;
// case CHASSIS_MODE_OPEN:
// out_current = c->setpoint.motor_rpm[i] / 7000.0f;
// break;
// case CHASSIS_MODE_RELAX:
// out_current = 0.0f;
// break;
// }
// //低通滤波和限幅
// c->out.motor[i] = LowPassFilter2p_Apply(&c->filter.out[i], out_current);
// Clip(&c->out.motor[i], -c->param->limit.max_current, c->param->limit.max_current);
// }
// return CHASSIS_OK;
// }
// /**
// * @brief 电机输出
// * @param c 底盘结构体指针
// */
// void Chassis_Output(Chassis_t *c) {
// if (!c)
// return;
// //每个电机目标输出
// for (uint8_t i = 0; i < c->num_wheel; i++) {
// MOTOR_RM_t *rm = c->motors[i];
// if (!rm) continue;
// MOTOR_RM_SetOutput(&rm->param, c->out.motor[i]);
// }
// //调用ctrl
// for (uint8_t i = 0; i < c->num_wheel; i++) {
// MOTOR_RM_t *rm = c->motors[i];
// if (rm) {
// MOTOR_RM_Ctrl(&rm->param);
// }
// }
// }
// /**
// * @brief 重置底盘输出
// * @param c 底盘结构体指针
// */
// void Chassis_ResetOutput(Chassis_t *c) {
// if (!c) return;
// for (uint8_t i = 0; i < c->num_wheel; i++) {
// MOTOR_RM_t *m = c->motors[i];
// if (m) {
// MOTOR_RM_Relax(&(m->param));
// }
// }
// }

238
User/module/chassis_hahaha.h
View File

@ -1,238 +0,0 @@
// /*
// 底盘模组
// */
// #pragma once
// #ifdef __cplusplus
// extern "C" {
// #endif
// /* Includes ----------------------------------------------------------------- */
// #include <cmsis_os2.h>
// #include "bsp/can.h"
// #include "component\filter.h"
// #include "component\mixer.h"
// #include "component\pid.h"
// #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 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_t;
// /* 小陀螺转动模式 */
// typedef enum {
// 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_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_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参数 */
// } pid;
// Chassis_Type_t type; /* 底盘类型，底盘的机械设计和轮子选型 */
// /* 低通滤波器截至频率*/
// struct {
// float in; /* 输入 */
// float out; /* 输出 */
// } low_pass_cutoff_freq;
// /* 电机反装，应该和云台设置相同*/
// struct {
// bool yaw;
// } reverse;
// struct {
// float max_vx, max_vy, max_wz;
// float max_current;
// } limit;
// } Chassis_Params_t;
// typedef struct {
// AHRS_Gyro_t gyro;
// AHRS_Eulr_t eulr;
// } Chassis_IMU_t;
// typedef struct {
// MOTOR_Feedback_t motor[4]; // 四个 3508电机 反馈
// float encoder_gimbalYawMotor;
// } Chassis_Feedback_t;
// /* 底盘输出结构体*/
// typedef struct {
// float motor[4];
// } Chassis_Output_t;
// /*
// * 运行的主结构体￣
// * 包含初始化参数,中间变量,输出变量
// */
// typedef struct {
// uint64_t last_wakeup;
// float dt;
// Chassis_Params_t *param; /* 底盘参数,用Chassis_Init设定 */
// /* 模块通用 */
// 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; /* 小陀螺旋转模式 */
// /* PID计算目标值 */
// struct {
// float motor_rpm[4]; /* 电机转速的动态数组,单位:RPM */
// } setpoint;
// /* 反馈控制用的PID */
// struct {
// 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]; /* 输出值滤波器 */
// } filter;
// Chassis_Output_t out; /* 电机输出 */
// Chassis_Feedback_t feedback;
// //float out_motor[4];
// } Chassis_t;
// /* Exported functions prototypes -------------------------------------------- */
// /**
// * \brief 底盘初始化
// *
// * \param c 包含底盘数据的结构体
// * \param param 包含底盘参数的结构体指针
// * \param target_freq 任务预期的运行频率
// *
// * \return 运行结果
// */
// int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
// float target_freq);
// /**
// * \brief 更新底盘反馈信息
// *
// * \param c 包含底盘数据的结构体
// * \param can CAN设备结构体
// *
// * \return 运行结果
// */
// int8_t Chassis_UpdateFeedback(Chassis_t *c);
// /**
// * \brief 运行底盘控制逻辑
// *
// * \param c 包含底盘数据的结构体
// * \param c_cmd 底盘控制指令
// * \param dt_sec 两次调用的时间间隔
// *
// * \return 运行结果
// */
// int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd,
// uint32_t now);
// /**
// * \brief 复制底盘输出值
// *
// * \param s 包含底盘数据的结构体
// * \param out CAN设备底盘输出结构体
// */
// void Chassis_Output(Chassis_t *c);
// /**
// * \brief 清空Chassis输出数据
// *
// * \param out CAN设备底盘输出结构体
// */
// void Chassis_ResetOutput(Chassis_t *c);
// /**
// * @brief 底盘功率限制
// *
// * @param c 底盘数据
// * @param cap 电容数据
// * @param ref 裁判系统数据
// * @return 函数运行结果
// */
// //还没有加入，waiting。。。。。。int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap,
// // const Referee_ForChassis_t *ref);
// /**
// * @brief 导出底盘数据
// *
// * @param chassis 底盘数据结构体
// * @param ui UI数据结构体
// */
// //void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);
// #ifdef __cplusplus
// }
// #endif

6
User/module/cmd_v2/cmd.h
View File

@ -12,6 +12,7 @@
#include "module/chassis.h"
#include "module/gimbal.h"
#include "module/shoot.h"
#include "module/track.h"
#include <stdint.h>
#ifdef __cplusplus
@ -38,6 +39,11 @@ typedef struct {
Shoot_CMD_t cmd;
} CMD_ShootOutput_t;
typedef struct {
CMD_InputSource_t source;
Track_CMD_t cmd;
} CMD_TrackOutput_t;
/* ========================================================================== */
/* 配置结构 */
/* ========================================================================== */

1
User/module/cmd_v2/cmd_behavior.c
View File

@ -3,7 +3,6 @@
*/
#include "cmd_behavior.h"
#include "cmd.h"
#include "module/gimbal.h"
#include <string.h>
/* ========================================================================== */

112
User/module/config.c
View File

@ -93,46 +93,7 @@ Config_RobotParam_t robot_config = {
.max_current = 16000.0f
},
},
.track_param = {
.motor = {
{
.can = BSP_CAN_1,
.id = 0x205,
.module = MOTOR_M3508,
.reverse = false,
.gear = true,
},
{
.can = BSP_CAN_1,
.id = 0x206,
.module = MOTOR_M3508,
.reverse = true,
.gear = true,
},
},
.pid = {
{
.k = 0.001f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
{
.k = 0.001f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
},
},
.gimbal_param = {
.pid = {
.yaw_omega = {
@ -363,36 +324,6 @@ Config_RobotParam_t robot_config = {
},
},
},
// .cmd_param={
// .sourcePriorityConfigs={
// CMD_SRC_RC,
// CMD_SRC_PC,
// CMD_SRC_NUC,
// CMD_SRC_REF
// },
// .pc.map = { /*1<<CMD_KEY_W记录一下不要忘了这个思路 */
// .key_map[CMD_BEHAVIOR_FORE] = {CMD_KEY_W, CMD_ACTIVE_PRESSED},
// .key_map[CMD_BEHAVIOR_BACK] = {CMD_KEY_S, CMD_ACTIVE_PRESSED},
// .key_map[CMD_BEHAVIOR_LEFT] = {CMD_KEY_A, CMD_ACTIVE_PRESSED},
// .key_map[CMD_BEHAVIOR_RIGHT] = {CMD_KEY_D, CMD_ACTIVE_PRESSED},
// .key_map[CMD_BEHAVIOR_ACCELERATE] = {CMD_KEY_SHIFT, CMD_ACTIVE_PRESSED},
// .key_map[CMD_BEHAVIOR_DECELEBRATE] = {CMD_KEY_CTRL, CMD_ACTIVE_PRESSED},
// .key_map[CMD_BEHAVIOR_FIRE] = {CMD_L_CLICK, CMD_ACTIVE_PRESSED},
// .key_map[CMD_BEHAVIOR_FIRE_MODE] = {CMD_R_CLICK, CMD_ACTIVE_RISING_EDGE},
// .key_map[CMD_BEHAVIOR_FOLLOWGIMBAL35] = {CMD_KEY_E, CMD_ACTIVE_RISING_EDGE},
// .key_map[CMD_BEHAVIOR_OPENCOVER] = {CMD_KEY_F, CMD_ACTIVE_RISING_EDGE},
// .key_map[CMD_BEHAVIOR_REVTRIG] = {CMD_KEY_R, CMD_ACTIVE_RISING_EDGE},
// .key_map[CMD_BEHAVIOR_ROTOR] = {CMD_KEY_G, CMD_ACTIVE_PRESSED},
// .key_map[CMD_BEHAVIOR_GIMBAL_MODE] = {CMD_KEY_V, CMD_ACTIVE_RISING_EDGE},
// },
// .pc.sensitivity={
// .move_sense=0.1f,
// .sens_mouse=3.0f,
// .move_fast_sense=2.4f,
// .move_slow_sense=0.8f
// },
// },
.cmd_param={
.source_priority = {
CMD_SRC_RC,
@ -415,7 +346,46 @@ Config_RobotParam_t robot_config = {
.gimbal_sw_down = GIMBAL_MODE_RELATIVE,
},
},
.track_param={
.motor = {
{
.can = BSP_CAN_1,
.id = 0x205,
.module = MOTOR_M3508,
.reverse = false,
.gear = true,
},
{
.can = BSP_CAN_1,
.id = 0x206,
.module = MOTOR_M3508,
.reverse = true,
.gear = true,
},
},
.pid = {
{
.k = 0.001f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
{
.k = 0.001f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
},
},
};
power_model_t cha=

63
User/module/track.c
View File

@ -5,22 +5,26 @@
/* Includes ----------------------------------------------------------------- */
#include <math.h>
#include <stdint.h>
#include <string.h>
#include "track.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/filter.h"
#include "component/user_math.h"
#include "module/chassis.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
#define WHEEL_RADIUS 0.03f //驱动轮半径，单位米
#define TRACK_RADIUS 1.0f //履带驱动轮半径，单位米
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
uint8_t TRACK_Init(Track_t *t, Track_Params_t *param, float target_freq)
uint8_t Track_Init(Track_t *t, Track_Params_t *param, float target_freq)
{
if (t == NULL || param == NULL) {
return TRACK_ERR_NULL; // 参数错误
@ -33,7 +37,7 @@ uint8_t TRACK_Init(Track_t *t, Track_Params_t *param, float target_freq)
return TRACK_OK;
}
uint8_t TRACK_Control(Track_t *t, Track_CMD_t *cmd)
uint8_t Track_Control(Track_t *t, Track_CMD_t *cmd)
{
if (t == NULL || cmd == NULL) {
return TRACK_ERR_NULL; // 参数错误
@ -43,10 +47,59 @@ uint8_t TRACK_Control(Track_t *t, Track_CMD_t *cmd)
t->timer.lask_wakeup = BSP_TIME_Get_us();
for (int i=0; i<TRACK_MOTOR_NUM; i++) {
t->output.iout[i] = PID_Calc(&t->pid[i], cmd->vel, t->motor.feedback.rotor_speed, 0.0f, t->timer.dt);
MOTOR_RM_SetOutput(&t->param->motor[i], t->output.iout[i]);
t->output.currentOut[i] = PID_Calc(&t->pid[i], cmd->vel, t->motor[i].feedback.rotor_speed, 0.0f, t->timer.dt);
}
return TRACK_OK;
}
uint8_t Track_AutoControl(Track_t *t, const Chassis_t *chassis)
{
if (t == NULL) {
return TRACK_ERR_NULL; // 参数错误
}
t->timer.now = BSP_TIME_Get_us()/1000000.0f;
t->timer.dt = t->timer.now - t->timer.lask_wakeup/1000000.0f;
t->timer.lask_wakeup = BSP_TIME_Get_us();
if (chassis->mode==CHASSIS_MODE_FOLLOW_GIMBAL||chassis->mode==CHASSIS_MODE_FOLLOW_GIMBAL_35)
{
float target_rpm[2];
for (int i=0; i<TRACK_MOTOR_NUM; i++) {
float rpm = (chassis->feedback.motor[i].rotor_speed + chassis->feedback.motor[i+1].rotor_speed)/2;
target_rpm[i] = rpm*(WHEEL_RADIUS/TRACK_RADIUS);
t->output.currentOut[i] = PID_Calc(&t->pid[i], target_rpm[i], t->motor[i].feedback.rotor_speed, 0.0f, t->timer.dt);
}
} else {
for (int i=0; i<TRACK_MOTOR_NUM; i++) {
t->output.currentOut[i] = 0.0f;
}
PID_Reset(t->pid);
}
return TRACK_OK;
}
uint8_t Track_UpdateFeedback(Track_t *t)
{
if (t == NULL) {
return TRACK_ERR_NULL; // 参数错误
}
for (int i=0; i<TRACK_MOTOR_NUM; i++) {
MOTOR_RM_Update(&t->param->motor[i]);
MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&t->param->motor[i]);
t->motor[i] = *rm_motor;
}
return TRACK_OK;
}
uint8_t Track_Output(Track_t *t)
{
if (t == NULL) {
return TRACK_ERR_NULL; // 参数错误
}
for (int i=0; i<TRACK_MOTOR_NUM; i++) {
MOTOR_RM_SetOutput(&t->param->motor[i], t->output.currentOut[i]);
MOTOR_RM_Ctrl(&t->param->motor[i]);
}
MOTOR_RM_Ctrl(&t->param->motor[0]);
return TRACK_OK;
}

12
User/module/track.h
View File

@ -6,6 +6,7 @@
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
@ -36,6 +37,7 @@ typedef struct {
} Track_Params_t;
typedef struct {
bool enable;
float vel;
} Track_CMD_t;
@ -46,16 +48,18 @@ typedef struct {
KPID_t pid[TRACK_MOTOR_NUM];
MOTOR_RM_t motor;
MOTOR_RM_t motor[TRACK_MOTOR_NUM];
struct {
float iout[TRACK_MOTOR_NUM];
float currentOut[TRACK_MOTOR_NUM];
}output;
} Track_t;
/* Exported functions prototypes -------------------------------------------- */
uint8_t Track_Init(Track_t *t, Track_Params_t *param, float target_freq);
uint8_t Track_UpdateFeedback(Track_t *t);
uint8_t Track_AutoControl(Track_t *t, const Chassis_t *chassis);
uint8_t Track_Output(Track_t *t);
#ifdef __cplusplus
}
#endif

9
User/task/ctrl_chassis.c
View File

@ -9,6 +9,7 @@
#include "module/chassis.h"
#include "module/config.h"
#include "device/supercap.h"
#include "module/track.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -20,6 +21,8 @@ Chassis_t chassis;
Chassis_CMD_t chassis_cmd;
Chassis_IMU_t chassis_imu;
float max =50;
Track_t track;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -38,6 +41,7 @@ void Task_ctrl_chassis(void *argument) {
Config_RobotParam_t *cfg = Config_GetRobotParam();
Chassis_Init(&chassis, &cfg->chassis_param, (float)CTRL_CHASSIS_FREQ);
Track_Init(&track, &cfg->track_param, (float)CTRL_CHASSIS_FREQ);
chassis.mech_zero=0.57f;
/* USER CODE INIT END */
@ -54,6 +58,11 @@ void Task_ctrl_chassis(void *argument) {
Chassis_Power_Control(&chassis,max);
Chassis_Output(&chassis);
Track_UpdateFeedback(&track);
Track_AutoControl(&track,&chassis);
Track_Output(&track);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}