From 407816a5a2ede2d85dae7b9461ce5348c8d5022b Mon Sep 17 00:00:00 2001 From: yxming66 <2389287465@qq.com> Date: Sun, 28 Dec 2025 14:20:06 +0800 Subject: [PATCH] 111 --- CMakeLists.txt | 2 + User/module/chassis.c | 2 - User/module/chassis_hahaha.c | 299 ------------------------------ User/module/chassis_hahaha.h | 238 ------------------------ User/module/cmd_v2/cmd.h | 6 + User/module/cmd_v2/cmd_behavior.c | 1 - User/module/config.c | 112 ++++------- User/module/track.c | 63 ++++++- User/module/track.h | 12 +- User/task/ctrl_chassis.c | 9 + 10 files changed, 124 insertions(+), 620 deletions(-) delete mode 100644 User/module/chassis_hahaha.c delete mode 100644 User/module/chassis_hahaha.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 993ec50..6c47959 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/User/module/chassis.c b/User/module/chassis.c index 31369c0..ceb9569 100644 --- a/User/module/chassis.c +++ b/User/module/chassis.c @@ -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]; diff --git a/User/module/chassis_hahaha.c b/User/module/chassis_hahaha.c deleted file mode 100644 index 0b60898..0000000 --- a/User/module/chassis_hahaha.c +++ /dev/null @@ -1,299 +0,0 @@ -// /* -// 底盘模组 -// */ - -// /* -// 底盘模组 -// */ - -// #include "cmsis_os2.h" -// #include -// #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, ¶m->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, ¶m->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)); -// } -// } -// } - diff --git a/User/module/chassis_hahaha.h b/User/module/chassis_hahaha.h deleted file mode 100644 index 408a00e..0000000 --- a/User/module/chassis_hahaha.h +++ /dev/null @@ -1,238 +0,0 @@ -// /* -// 底盘模组 -// */ -// #pragma once - - -// #ifdef __cplusplus -// extern "C" { -// #endif - -// /* Includes ----------------------------------------------------------------- */ -// #include -// #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 - - - - diff --git a/User/module/cmd_v2/cmd.h b/User/module/cmd_v2/cmd.h index c70c46f..b135cbb 100644 --- a/User/module/cmd_v2/cmd.h +++ b/User/module/cmd_v2/cmd.h @@ -12,6 +12,7 @@ #include "module/chassis.h" #include "module/gimbal.h" #include "module/shoot.h" +#include "module/track.h" #include #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; + /* ========================================================================== */ /* 配置结构 */ /* ========================================================================== */ diff --git a/User/module/cmd_v2/cmd_behavior.c b/User/module/cmd_v2/cmd_behavior.c index 1b0773f..3f18a8c 100644 --- a/User/module/cmd_v2/cmd_behavior.c +++ b/User/module/cmd_v2/cmd_behavior.c @@ -3,7 +3,6 @@ */ #include "cmd_behavior.h" #include "cmd.h" -#include "module/gimbal.h" #include /* ========================================================================== */ diff --git a/User/module/config.c b/User/module/config.c index 8b5baef..ea04c85 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -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< +#include #include #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; ioutput.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; ifeedback.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; ioutput.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; iparam->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; iparam->motor[i], t->output.currentOut[i]); + MOTOR_RM_Ctrl(&t->param->motor[i]); } - MOTOR_RM_Ctrl(&t->param->motor[0]); return TRACK_OK; } diff --git a/User/module/track.h b/User/module/track.h index 958e499..d5a4a2e 100644 --- a/User/module/track.h +++ b/User/module/track.h @@ -6,6 +6,7 @@ #include #include +#include #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 diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c index aaae4c1..a3b2bbf 100644 --- a/User/task/ctrl_chassis.c +++ b/User/task/ctrl_chassis.c @@ -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); /* 运行结束，等待下一次唤醒 */ }