111
This commit is contained in:
parent
e1e8329d28
commit
407816a5a2
@ -42,6 +42,7 @@ target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE
|
|||||||
# Add user defined library search paths
|
# Add user defined library search paths
|
||||||
)
|
)
|
||||||
file(GLOB SINGLE_CHAR_FILES "User/module/cmd_v2/*.c")
|
file(GLOB SINGLE_CHAR_FILES "User/module/cmd_v2/*.c")
|
||||||
|
|
||||||
# Add sources to executable
|
# Add sources to executable
|
||||||
target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||||
# Add user sources here
|
# Add user sources here
|
||||||
@ -78,6 +79,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
|||||||
User/module/gimbal.c
|
User/module/gimbal.c
|
||||||
User/module/shoot.c
|
User/module/shoot.c
|
||||||
${SINGLE_CHAR_FILES}
|
${SINGLE_CHAR_FILES}
|
||||||
|
User/module/track.c
|
||||||
# User/task sources
|
# User/task sources
|
||||||
User/task/atti_esti.c
|
User/task/atti_esti.c
|
||||||
User/task/blink.c
|
User/task/blink.c
|
||||||
|
|||||||
@ -283,8 +283,6 @@ void Chassis_Output(Chassis_t *c) {
|
|||||||
MOTOR_RM_SetOutput(&rm->param, c->out.motor[i]);
|
MOTOR_RM_SetOutput(&rm->param, c->out.motor[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//µ÷ÓÃctrl
|
//µ÷ÓÃctrl
|
||||||
for (uint8_t i = 0; i < c->num_wheel; i++) {
|
for (uint8_t i = 0; i < c->num_wheel; i++) {
|
||||||
MOTOR_RM_t *rm = c->motors[i];
|
MOTOR_RM_t *rm = c->motors[i];
|
||||||
|
|||||||
@ -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, ¶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));
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
|
|
||||||
@ -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
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -12,6 +12,7 @@
|
|||||||
#include "module/chassis.h"
|
#include "module/chassis.h"
|
||||||
#include "module/gimbal.h"
|
#include "module/gimbal.h"
|
||||||
#include "module/shoot.h"
|
#include "module/shoot.h"
|
||||||
|
#include "module/track.h"
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
@ -38,6 +39,11 @@ typedef struct {
|
|||||||
Shoot_CMD_t cmd;
|
Shoot_CMD_t cmd;
|
||||||
} CMD_ShootOutput_t;
|
} CMD_ShootOutput_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
CMD_InputSource_t source;
|
||||||
|
Track_CMD_t cmd;
|
||||||
|
} CMD_TrackOutput_t;
|
||||||
|
|
||||||
/* ========================================================================== */
|
/* ========================================================================== */
|
||||||
/* 配置结构 */
|
/* 配置结构 */
|
||||||
/* ========================================================================== */
|
/* ========================================================================== */
|
||||||
|
|||||||
@ -3,7 +3,6 @@
|
|||||||
*/
|
*/
|
||||||
#include "cmd_behavior.h"
|
#include "cmd_behavior.h"
|
||||||
#include "cmd.h"
|
#include "cmd.h"
|
||||||
#include "module/gimbal.h"
|
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
|
||||||
/* ========================================================================== */
|
/* ========================================================================== */
|
||||||
|
|||||||
@ -93,46 +93,7 @@ Config_RobotParam_t robot_config = {
|
|||||||
.max_current = 16000.0f
|
.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 = {
|
.gimbal_param = {
|
||||||
.pid = {
|
.pid = {
|
||||||
.yaw_omega = {
|
.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={
|
.cmd_param={
|
||||||
.source_priority = {
|
.source_priority = {
|
||||||
CMD_SRC_RC,
|
CMD_SRC_RC,
|
||||||
@ -415,7 +346,46 @@ Config_RobotParam_t robot_config = {
|
|||||||
.gimbal_sw_down = GIMBAL_MODE_RELATIVE,
|
.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=
|
power_model_t cha=
|
||||||
|
|||||||
@ -5,22 +5,26 @@
|
|||||||
|
|
||||||
/* Includes ----------------------------------------------------------------- */
|
/* Includes ----------------------------------------------------------------- */
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
#include <stdint.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include "track.h"
|
#include "track.h"
|
||||||
#include "bsp/mm.h"
|
#include "bsp/mm.h"
|
||||||
#include "bsp/time.h"
|
#include "bsp/time.h"
|
||||||
#include "component/filter.h"
|
#include "component/filter.h"
|
||||||
#include "component/user_math.h"
|
#include "component/user_math.h"
|
||||||
|
#include "module/chassis.h"
|
||||||
/* Private typedef ---------------------------------------------------------- */
|
/* Private typedef ---------------------------------------------------------- */
|
||||||
/* Private define ----------------------------------------------------------- */
|
/* Private define ----------------------------------------------------------- */
|
||||||
|
|
||||||
/* Private macro ------------------------------------------------------------ */
|
/* Private macro ------------------------------------------------------------ */
|
||||||
|
#define WHEEL_RADIUS 0.03f //驱动轮半径，单位米
|
||||||
|
#define TRACK_RADIUS 1.0f //履带驱动轮半径，单位米
|
||||||
/* Private variables -------------------------------------------------------- */
|
/* Private variables -------------------------------------------------------- */
|
||||||
|
|
||||||
/* Private function -------------------------------------------------------- */
|
/* Private function -------------------------------------------------------- */
|
||||||
|
|
||||||
/* Exported functions ------------------------------------------------------- */
|
/* 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) {
|
if (t == NULL || param == NULL) {
|
||||||
return TRACK_ERR_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;
|
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) {
|
if (t == NULL || cmd == NULL) {
|
||||||
return TRACK_ERR_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();
|
t->timer.lask_wakeup = BSP_TIME_Get_us();
|
||||||
|
|
||||||
for (int i=0; i<TRACK_MOTOR_NUM; i++) {
|
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);
|
t->output.currentOut[i] = PID_Calc(&t->pid[i], cmd->vel, t->motor[i].feedback.rotor_speed, 0.0f, t->timer.dt);
|
||||||
MOTOR_RM_SetOutput(&t->param->motor[i], t->output.iout[i]);
|
}
|
||||||
|
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;
|
return TRACK_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -6,6 +6,7 @@
|
|||||||
|
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <stddef.h>
|
#include <stddef.h>
|
||||||
|
#include <stdint.h>
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
extern "C" {
|
extern "C" {
|
||||||
#endif
|
#endif
|
||||||
@ -36,6 +37,7 @@ typedef struct {
|
|||||||
} Track_Params_t;
|
} Track_Params_t;
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
|
|
||||||
bool enable;
|
bool enable;
|
||||||
float vel;
|
float vel;
|
||||||
} Track_CMD_t;
|
} Track_CMD_t;
|
||||||
@ -46,16 +48,18 @@ typedef struct {
|
|||||||
|
|
||||||
KPID_t pid[TRACK_MOTOR_NUM];
|
KPID_t pid[TRACK_MOTOR_NUM];
|
||||||
|
|
||||||
MOTOR_RM_t motor;
|
MOTOR_RM_t motor[TRACK_MOTOR_NUM];
|
||||||
|
|
||||||
struct {
|
struct {
|
||||||
float iout[TRACK_MOTOR_NUM];
|
float currentOut[TRACK_MOTOR_NUM];
|
||||||
}output;
|
}output;
|
||||||
} Track_t;
|
} Track_t;
|
||||||
|
|
||||||
/* Exported functions prototypes -------------------------------------------- */
|
/* 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
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -9,6 +9,7 @@
|
|||||||
#include "module/chassis.h"
|
#include "module/chassis.h"
|
||||||
#include "module/config.h"
|
#include "module/config.h"
|
||||||
#include "device/supercap.h"
|
#include "device/supercap.h"
|
||||||
|
#include "module/track.h"
|
||||||
/* USER INCLUDE END */
|
/* USER INCLUDE END */
|
||||||
|
|
||||||
/* Private typedef ---------------------------------------------------------- */
|
/* Private typedef ---------------------------------------------------------- */
|
||||||
@ -20,6 +21,8 @@ Chassis_t chassis;
|
|||||||
Chassis_CMD_t chassis_cmd;
|
Chassis_CMD_t chassis_cmd;
|
||||||
Chassis_IMU_t chassis_imu;
|
Chassis_IMU_t chassis_imu;
|
||||||
float max =50;
|
float max =50;
|
||||||
|
|
||||||
|
Track_t track;
|
||||||
/* USER STRUCT END */
|
/* USER STRUCT END */
|
||||||
|
|
||||||
/* Private function --------------------------------------------------------- */
|
/* Private function --------------------------------------------------------- */
|
||||||
@ -38,6 +41,7 @@ void Task_ctrl_chassis(void *argument) {
|
|||||||
|
|
||||||
Config_RobotParam_t *cfg = Config_GetRobotParam();
|
Config_RobotParam_t *cfg = Config_GetRobotParam();
|
||||||
Chassis_Init(&chassis, &cfg->chassis_param, (float)CTRL_CHASSIS_FREQ);
|
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;
|
chassis.mech_zero=0.57f;
|
||||||
/* USER CODE INIT END */
|
/* USER CODE INIT END */
|
||||||
|
|
||||||
@ -54,6 +58,11 @@ void Task_ctrl_chassis(void *argument) {
|
|||||||
Chassis_Power_Control(&chassis,max);
|
Chassis_Power_Control(&chassis,max);
|
||||||
|
|
||||||
Chassis_Output(&chassis);
|
Chassis_Output(&chassis);
|
||||||
|
|
||||||
|
Track_UpdateFeedback(&track);
|
||||||
|
Track_AutoControl(&track,&chassis);
|
||||||
|
Track_Output(&track);
|
||||||
|
|
||||||
/* USER CODE END */
|
/* USER CODE END */
|
||||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user