diff --git a/CMakeLists.txt b/CMakeLists.txt index 56977f2..d547091 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,7 +76,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/module/config.c User/module/gimbal.c User/module/shoot.c - + User/module/cmd.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 c499d01..b3ea0e8 100644 --- a/User/module/chassis.c +++ b/User/module/chassis.c @@ -1,208 +1,299 @@ +/* +底盘模组 +*/ -#include "chassis.h" +/* +底盘模组 +*/ -#include "can.h" -#include "device/at9s_pro.h" -#include "component/pid.h" +#include "cmsis_os2.h" +#include +#include "bsp/mm.h" +#include "bsp/can.h" +#include "component/ahrs.h" #include "device/motor_rm.h" -#include -#include "bsp/time.h" +#include "device/motor.h" +#include "module/chassis.h" -KPID_t pid_wheel[4]; -KPID_Params_t pid_wheel_params={1, 0.8, 0.0, 0.0, 0.0, 0.8,-1.0,-1.0}; - -KPID_t pid_lockagl[4]; -KPID_Params_t pid_lockagl_params={1, 0.8, 0.05, 0.0, 0.1, 0.8,-1.0,M_2PI}; - -KPID_t pid_lockomg[4]; -KPID_Params_t pid_lockomg_params={2, 1, 0.0, 0.01, 0.0, 0.8,-1.0,-1.0}; +/** + * @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 //角速度变化频率 -KPID_t pid_follow; -KPID_Params_t pid_follow_params={0.5, 1, 0, 0.0, 0, 0.0, -1.0, -1}; -MOTOR_RM_Param_t motor1to4_param[4]={ - {.can=BSP_CAN_1,.id=0x201,.module=MOTOR_M3508,.reverse=false,.gear=true,}, - {.can=BSP_CAN_1,.id=0x202,.module=MOTOR_M3508,.reverse=false,.gear=true,}, - {.can=BSP_CAN_1,.id=0x203,.module=MOTOR_M3508,.reverse=false,.gear=true,}, - {.can=BSP_CAN_1,.id=0x204,.module=MOTOR_M3508,.reverse=false,.gear=true,} -}; +/** + * @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); + } -MOTOR_Feedback_t motor1to4_measure[4]; -float lockagl_out[4]; -float chassis_out[4]; -float Wheel_status[4]; -void Inverse_resolve(float vx,float vy,float w) -{ - Wheel_status[0]=vx-vy+w; - Wheel_status[1]=vx+vy+w; - Wheel_status[2]=-vx+vy+w; - Wheel_status[3]=-vx-vy+w; + c->mode = mode; + return CHASSIS_OK; } -void Motor_Init(float freq) -{ for(int i=0;i<4;i++) - { - PID_Init(&pid_wheel[i], KPID_MODE_CALC_D, freq,&pid_wheel_params); - PID_Init(&pid_lockagl[i], KPID_MODE_CALC_D, freq,&pid_lockagl_params); - PID_Init(&pid_lockomg[i], KPID_MODE_CALC_D, freq,&pid_lockomg_params); +/** + * @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; +} - } - - - PID_Init(&pid_follow, KPID_MODE_NO_D, freq,&pid_follow_params); - BSP_CAN_Init(); - for(int i=0;i<4;i++){ - MOTOR_RM_Register(&motor1to4_param[i]); +/** + * @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); + } } } -int64_t now,last_wakeup; -float dt; -float vx,vy,w;float chassis_yaw; -float speed[4]; -float lockagl[4]={0,0,0,0}; -void chassis_control(chassis_ctrl_eulr_t eulr, COMP_AT9S_CMD_t cmd_rc)//k:ת���޷� -{ - //float vx,vy,w; - now =BSP_TIME_Get_us() / 1000000.0f; - dt =(BSP_TIME_Get_us() - last_wakeup) / 1000000.0f; - last_wakeup =BSP_TIME_Get_us(); - float delta_angle; - float cos_delta_angle; - float sin_delta_angle; - for(int i=0;i<4;i++){ - MOTOR_RM_Update(&motor1to4_param[i]); - MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&motor1to4_param[i]); - if(motor_fed!=NULL) - { - motor1to4_measure[i]=motor_fed->motor.feedback; - } - } - switch(cmd_rc.mode){ - case 0: - vx=0;vy=0;w=0; - break; - case 1: - delta_angle = eulr.chassis_encoder_yaw - eulr.chassis_mech_zero_yaw; - cos_delta_angle = cosf(delta_angle); - sin_delta_angle = sinf(delta_angle); - vx =cos_delta_angle * cmd_rc.chassis_ctrl_vec.x - sin_delta_angle * cmd_rc.chassis_ctrl_vec.y; - vy =sin_delta_angle * cmd_rc.chassis_ctrl_vec.x + cos_delta_angle * cmd_rc.chassis_ctrl_vec.y; - w=PID_Calc(&pid_follow, eulr.chassis_mech_zero_yaw, - eulr.chassis_encoder_yaw, 0.0f, dt); - break; - case 2: - PID_ResetIntegral(&pid_follow); - - delta_angle = eulr.chassis_encoder_yaw - eulr.chassis_mech_zero_yaw; - cos_delta_angle = cosf(delta_angle); - sin_delta_angle = sinf(delta_angle); - vx =cos_delta_angle * cmd_rc.chassis_ctrl_vec.x - sin_delta_angle * cmd_rc.chassis_ctrl_vec.y; - vy =sin_delta_angle * cmd_rc.chassis_ctrl_vec.x + cos_delta_angle * cmd_rc.chassis_ctrl_vec.y; - - w=PID_Calc(&pid_follow, eulr.chassis_mech_zero_yaw, - eulr.chassis_encoder_yaw, 0.0f, dt); - break; - case 3: - delta_angle = eulr.chassis_encoder_yaw - eulr.chassis_mech_zero_yaw; - cos_delta_angle = cosf(delta_angle); - sin_delta_angle = sinf(delta_angle); - vx =cos_delta_angle * cmd_rc.chassis_ctrl_vec.x - sin_delta_angle * cmd_rc.chassis_ctrl_vec.y; - vy =sin_delta_angle * cmd_rc.chassis_ctrl_vec.x + cos_delta_angle * cmd_rc.chassis_ctrl_vec.y; - - w=0.6f; - break; - default : - break; - } - - - Inverse_resolve(vx, vy, w); - float abs_max = 0.0f; - for (int8_t i = 0; i < 4; i++) { - const float abs_val = fabsf(Wheel_status[i]); - abs_max = (abs_val > abs_max) ? abs_val : abs_max; - } - if (abs_max > 1.0f) { - for (int8_t i = 0; i < 4; i++) { - Wheel_status[i] /= abs_max; - } - } - -// if(vx==0&&vy==0)PID_ResetIntegral(&pid_wheel); - switch(cmd_rc.mode){ - case 0: - PID_ResetIntegral(&pid_wheel[0]); - PID_ResetIntegral(&pid_wheel[1]); - PID_ResetIntegral(&pid_wheel[2]); - PID_ResetIntegral(&pid_wheel[3]); - chassis_out[0] = 0; - chassis_out[1] = 0; - chassis_out[2] = 0; - chassis_out[3] = 0; - break; - case 1: - PID_ResetIntegral(&pid_wheel[0]); - PID_ResetIntegral(&pid_wheel[1]); - PID_ResetIntegral(&pid_wheel[2]); - PID_ResetIntegral(&pid_wheel[3]); -// chassis_out[0] = 0; -// chassis_out[1] = 0; -// chassis_out[2] = 0; -// chassis_out[3] = 0; - for(int i=0;i<4;i++){ - speed[i]=motor1to4_measure[i].rotor_speed*19.2032/8000.0f; - - if(speed[i]>1)speed[i]=1; - else if(speed[i]<-1)speed[i]=-1; - lockagl_out[i] = PID_Calc(&pid_lockagl[i],lockagl[i],motor1to4_measure[i].rotor_abs_angle, 0,dt); - chassis_out[i] = PID_Calc(&pid_lockomg[i],lockagl_out[i],speed[i], 0,dt); - if(chassis_out[i]>0.8f)chassis_out[i]=0.8f; - } - break; - case 2: - for(int i=0;i<4;i++){ - - lockagl[i]=motor1to4_measure[i].rotor_abs_angle; - - speed[i]=motor1to4_measure[i].rotor_speed*19.2032/8000.0f; - if(speed[i]>1)speed[i]=1; - else if(speed[i]<-1)speed[i]=-1; - chassis_out[i] = PID_Calc(&pid_wheel[i],Wheel_status[i],speed[i], 0,dt); - - } - - break; - case 3: - for(int i=0;i<4;i++){ - - lockagl[i]=motor1to4_measure[i].rotor_abs_angle; - - speed[i]=motor1to4_measure[i].rotor_speed*19.2032/8000.0f; - if(speed[i]>1)speed[i]=1; - else if(speed[i]<-1)speed[i]=-1; - chassis_out[i] = PID_Calc(&pid_wheel[i],Wheel_status[i],speed[i], 0,dt); - - } - break; - default : - break; - } -// for(int i;i<4;i++){ -// chassis_out[i]*=0.6; -// } - MOTOR_RM_SetOutput(&motor1to4_param[0], chassis_out[0]); - MOTOR_RM_SetOutput(&motor1to4_param[1], chassis_out[1]); - MOTOR_RM_SetOutput(&motor1to4_param[2], chassis_out[2]); - MOTOR_RM_SetOutput(&motor1to4_param[3], chassis_out[3]); -//MOTOR_RM_SetOutput(&motor1to4_param[1], 0); -// MOTOR_RM_SetOutput(&motor1to4_param[2], 0); - MOTOR_RM_Ctrl(&motor1to4_param[0]); +/** + * @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.h b/User/module/chassis.h index 446f234..9b9034f 100644 --- a/User/module/chassis.h +++ b/User/module/chassis.h @@ -1,21 +1,238 @@ +/* + 底盘模组 + */ +#pragma once -#ifndef CHASSIS_CONTROL_H -#define CHASSIS_CONTROL_H +#ifdef __cplusplus +extern "C" { +#endif -#include "main.h" +/* 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; -#include "component/at9s_pro_cmd.h" -/* ŷ���ǣ�Euler angle�� */ +/* 底盘控制命令 */ typedef struct { - float chassis_mech_zero_yaw; - float chassis_encoder_yaw; -} chassis_ctrl_eulr_t; + Chassis_Mode_t mode; /* 底盘运行模式 */ + Chassis_RotorMode_t mode_rotor; /* 小陀螺转动模式 */ + MoveVector_t ctrl_vec; /* 底盘控制向量 */ +} Chassis_CMD_t; -void Inverse_resolve(float vx,float vy,float w); -void Motor_Init(float freq); -void chassis_control(chassis_ctrl_eulr_t eulr, COMP_AT9S_CMD_t cmd_rc); +/* 限位 */ +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.c b/User/module/cmd.c index 59b70f8..58318ef 100644 --- a/User/module/cmd.c +++ b/User/module/cmd.c @@ -2,18 +2,271 @@ 控制命令 */ #include "module/cmd.h" +#include #include -/*************************************************************************/ -/*********************************仲裁器**********************************/ -/*************************************************************************/ +/*************************************************************************************************************************************/ +/***************************************************************仲裁器****************************************************************/ +/*************************************************************************************************************************************/ + +/* Includes ----------------------------------------------------------------- */ +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* Private function -------------------------------------------------------- */ + +bool isREFOnline(CMD_t *c){return c->input_ref.online;} +bool isNUCOnline(CMD_t *c){return c->input_nuc.online;} +bool isRCOnline(CMD_t *c){return c->input_rc.online;} +bool isPCOnline(CMD_t *c){return c->input_pc.online;} +void PriorityConfigsRanking(CMD_Param_t *param) { + static bool init=false; + if(init) return; + for (int i = 0; i < CMD_SRC_NUM - 1; i++) { + for (int j = 0; j < CMD_SRC_NUM - i - 1; j++) { + if (param->rcSourcePriorityConfigs[j].priority < param->rcSourcePriorityConfigs[j + 1].priority) { + CMD_RCSourcePriorityConfig_t temp = param->rcSourcePriorityConfigs[j]; + param->rcSourcePriorityConfigs[j] = param->rcSourcePriorityConfigs[j + 1]; + param->rcSourcePriorityConfigs[j + 1] = temp; + } + } + } + init=!init; +} +CMD_InputSource_t getHighestPrioritySource(CMD_t *c) { + for (int i = 0; i < CMD_SRC_NUM; i++) { + CMD_InputSource_t source = c->params.rcSourcePriorityConfigs[i].source; + switch (source) { + case CMD_SRC_REFEREE: + if (isREFOnline(c)) { + return CMD_SRC_REFEREE; + } + break; + case CMD_SRC_NUC: + if (isNUCOnline(c)) { + return CMD_SRC_NUC; + } + break; + case CMD_SRC_RC: + if (isRCOnline(c)) { + return CMD_SRC_RC; + } + break; + case CMD_SRC_PC: + if (isPCOnline(c)) { + return CMD_SRC_PC; + } + } + } + return CMD_SRC_PC; /*默认使用键盘鼠标*/ +} + +int8_t Cmd_Arbiter(CMD_t *c) { + if (c == NULL) { + return CMD_ERR_NULL; // 参数错误 + } + + PriorityConfigsRanking(&c->params); + + CMD_InputSource_t source = getHighestPrioritySource(c); + + c->out_chassis.source = source; + c->out_gimbal.source = source; + c->out_shoot.source = source; + + return CMD_OK; +} +/* Exported functions ------------------------------------------------------- */ +/*************************************************************************************************************************************/ +/****************************************************************RC*******************************************************************/ +/*************************************************************************************************************************************/ +/* API ---------------------------------------------------------------------- */ +/* Mrobot生成 */ +#define RC_SELECT_Index 1 +/* 扩展接口 */ +#if RC_SELECT_Index == 0 +#define FOR_EACH_RC(_) _(dr16, DR16) +#elif RC_SELECT_Index == 1 +#define FOR_EACH_RC(_) _(at9s, AT9S) +#endif +/* Includes ----------------------------------------------------------------- */ +#if RC_SELECT_Index == 0 +#include "device/dr16.h" +#elif RC_SELECT_Index == 1 +#include "device/at9s_pro.h" +#endif +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +#define RC_X_FIELD_MACRO(name, NAME) DEVICE_##NAME##_t name; +#define RC_X_EXTERN_MACRO(name, NAME) extern DEVICE_##NAME##_t name##_out; +#define RC_X_COPYDEFINE_MACRO(name, NAME) \ + static void copy_##name(rc_inputdata_u *dst) { dst->name = name##_out; } +#define RC_X_COPYREFERENCE_MACRO(name, NAME) copy_##name, +union rc_inputdata_u{ + FOR_EACH_RC(RC_X_FIELD_MACRO) +}; +FOR_EACH_RC(RC_X_EXTERN_MACRO) +FOR_EACH_RC(RC_X_COPYDEFINE_MACRO) +/* Private variables -------------------------------------------------------- */ +/*静态缓冲区*/ +static rc_inputdata_u rc_buffer; +/* Private function -------------------------------------------------------- */ +int8_t Cmd_Get_rc(CMD_InputData_RC_t *rc) +{ + FOR_EACH_RC(RC_X_COPYREFERENCE_MACRO)(&rc_buffer); + rc->inputData = &rc_buffer; + rc->type = RC_SELECT_Index; + return CMD_OK; +} -/*************************************************************************/ -/**********************************RC*************************************/ -/*************************************************************************/ + +int8_t Cmd_BuildChassisCommandFromInput_rc(CMD_t *c) { + if (c == NULL) { + return CMD_ERR_NULL; // 参数错误 + } +#if RC_SELECT_Index == 0 + c->input_rc.inputData->dr16.data = +#elif RC_SELECT_Index == 1 + switch (c->input_rc.inputData->at9s.data.key_E) { + case AT9S_CMD_SW_DOWN: + c->out_chassis.cmd.mode = CHASSIS_MODE_RELAX; + break; + case AT9S_CMD_SW_MID: + c->out_chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL; + break; + case AT9S_CMD_SW_UP: + c->out_chassis.cmd.mode = CHASSIS_MODE_ROTOR; + break; + default: + c->out_chassis.cmd.mode = CHASSIS_MODE_RELAX; + break; + } + c->out_chassis.cmd.ctrl_vec.vx = c->input_rc.inputData->at9s.data.ch_l_y; + c->out_chassis.cmd.ctrl_vec.vy = c->input_rc.inputData->at9s.data.ch_l_x; + c->out_chassis.cmd.ctrl_vec.wz = c->input_rc.inputData->at9s.data.ch_r_x; +#endif + return CMD_OK; +} + +int8_t Cmd_BuildGimbalCommandFromInput_rc(CMD_t *c) { + if (c == NULL) { + return CMD_ERR_NULL; // 参数错误 + } +#if RC_SELECT_Index == 0 + c->input_rc.inputData->dr16.data = +#elif RC_SELECT_Index == 1 + switch (c->input_rc.inputData->at9s.data.key_G) { + case AT9S_CMD_SW_DOWN: + c->out_gimbal.cmd.mode = GIMBAL_MODE_RELAX; + c->out_gimbal.cmd.delta_yaw = 0.0f; + c->out_gimbal.cmd.delta_pit = 0.0f; + break; + case AT9S_CMD_SW_MID: + c->out_gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE; + c->out_gimbal.cmd.delta_yaw = -at9s_out.data.ch_l_x * 2.0f; + c->out_gimbal.cmd.delta_pit = -at9s_out.data.ch_l_y * 1.5f; + break; + case AT9S_CMD_SW_UP: + c->out_gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE; + c->out_gimbal.cmd.delta_yaw = -at9s_out.data.ch_l_x * 2.0f; + c->out_gimbal.cmd.delta_pit = -at9s_out.data.ch_l_y * 1.5f; + break; + default: + c->out_gimbal.cmd.mode = GIMBAL_MODE_RELAX; + c->out_gimbal.cmd.delta_yaw = 0.0f; + c->out_gimbal.cmd.delta_pit = 0.0f; + break; + } +#endif + return CMD_OK; +} + + +int8_t Cmd_BuildShootCommandFromInput_rc(CMD_t *c) { + if (c == NULL) { + return CMD_ERR_NULL; // 参数错误 + } +#if RC_SELECT_Index == 0 + c->input_rc.inputData->dr16.data = +#elif RC_SELECT_Index == 1 + switch (c->input_rc.inputData->at9s.data.key_C) { + case AT9S_CMD_SW_DOWN: + c->out_shoot.cmd.online = at9s_out.online; + c->out_shoot.cmd.ready = true; + c->out_shoot.cmd.firecmd = true; + break; + case AT9S_CMD_SW_MID: + c->out_shoot.cmd.online = at9s_out.online; + c->out_shoot.cmd.ready = true; + c->out_shoot.cmd.firecmd = false; + break; + case AT9S_CMD_SW_UP: + c->out_shoot.cmd.online = at9s_out.online; + c->out_shoot.cmd.ready = false; + c->out_shoot.cmd.firecmd = false; + break; + default: + c->out_shoot.cmd.online = at9s_out.online; + c->out_shoot.cmd.ready = false; + c->out_shoot.cmd.firecmd = false; + break; + } +#endif + return CMD_OK; +} +/* Exported functions ------------------------------------------------------- */ + + +/*************************************************************************************************************************************/ +/*****************************************************************PC******************************************************************/ +/*************************************************************************************************************************************/ + +/* Includes ----------------------------------------------------------------- */ +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* Private function -------------------------------------------------------- */ +int8_t Cmd_Get_pc(CMD_InputData_PC_t *pc) +{ + FOR_EACH_RC(RC_X_COPYREFERENCE_MACRO)(&rc_buffer); + rc->inputData = &rc_buffer; + rc->type = RC_SELECT_Index; + return CMD_OK; +} +/* Exported functions ------------------------------------------------------- */ + + +int8_t Cmd_BuildChassisCommandFromInput_pc(CMD_t *c) { + if (c == NULL) { + return CMD_ERR_NULL; // 参数错误 + } + + return CMD_OK; +} + +int8_t Cmd_BuildGimbalCommandFromInput_pc(CMD_t *c) { + if (c == NULL) { + return CMD_ERR_NULL; // 参数错误 + } + + return CMD_OK; +} +int8_t Cmd_BuildShootCommandFromInput_pc(CMD_t *c) { + if (c == NULL) { + return CMD_ERR_NULL; // 参数错误 + } + + return CMD_OK; +} +/*************************************************************************************************************************************/ +/****************************************************************NUC******************************************************************/ +/*************************************************************************************************************************************/ /* Includes ----------------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */ @@ -23,63 +276,146 @@ /* Private function -------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */ -#define RC_SELECT_Index 1 +int8_t Cmd_BuildChassisCommandFromInput_nuc(CMD_t *c) { + if (c == NULL) { + return CMD_ERR_NULL; // 参数错误 + } -/* 扩展接口 */ -#if RC_SELECT_Index == 0 -#define FOR_EACH_RC(_) _(dr16, DR16) -#elif RC_SELECT_Index == 1 -#define FOR_EACH_RC(_) _(at9s, AT9S) -#endif - -#if RC_SELECT_Index == 0 -#include "device/dr16.h" -#elif RC_SELECT_Index == 1 -#include "device/at9s_pro.h" -#endif - -#define X_FIELD(name, NAME) DEVICE_##NAME##_t name; -#define X_EXTERN(name, NAME) extern DEVICE_##NAME##_t name##_out; -#define X_COPY(name, NAME) \ - static void copy_##name(rc_u *dst) { dst->name = name##_out; } -#define X_REF(name, NAME) copy_##name, - -union rc_u{ - FOR_EACH_RC(X_FIELD) -}; -FOR_EACH_RC(X_EXTERN) -FOR_EACH_RC(X_COPY) - -/*静态缓冲区，供返回使用*/ -static rc_u rc_buffer; - -CMD_RCInputData_t rc; - -int8_t Cmd_get_rc(CMD_RCInputData_t *dst) -{ - FOR_EACH_RC(X_REF)(&rc_buffer); - dst->rc = &rc_buffer; - dst->rc_type = RC_SELECT_Index; return CMD_OK; } -/*************************************************************************/ -/**********************************PC*************************************/ -/*************************************************************************/ +int8_t Cmd_BuildGimbalCommandFromInput_nuc(CMD_t *c) { + if (c == NULL) { + return CMD_ERR_NULL; // 参数错误 + } + return CMD_OK; +} +int8_t Cmd_BuildShootCommandFromInput_nuc(CMD_t *c) { + if (c == NULL) { + return CMD_ERR_NULL; // 参数错误 + } + return CMD_OK; +} -/*************************************************************************/ -/**********************************NUC*************************************/ -/*************************************************************************/ +/*************************************************************************************************************************************/ +/***************************************************************REF*******************************************************************/ +/*************************************************************************************************************************************/ +/* Includes ----------------------------------------------------------------- */ +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +int8_t Cmd_BuildChassisCommandFromInput_referee(CMD_t *c) { + if (c == NULL) { + return CMD_ERR_NULL; // 参数错误 + } -/*************************************************************************/ -/**********************************REF*************************************/ -/*************************************************************************/ + return CMD_OK; +} +int8_t Cmd_BuildGimbalCommandFromInput_referee(CMD_t *c) { + if (c == NULL) { + return CMD_ERR_NULL; // 参数错误 + } -/*************************************************************************/ -/*********************************分发命令*********************************/ -/*************************************************************************/ + return CMD_OK; +} +int8_t Cmd_BuildShootCommandFromInput_referee(CMD_t *c) { + if (c == NULL) { + return CMD_ERR_NULL; // 参数错误 + } + + return CMD_OK; +} + +/*************************************************************************************************************************************/ +/***************************************************************分发命令***************************************************************/ +/*************************************************************************************************************************************/ + +/* Includes ----------------------------------------------------------------- */ +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +typedef int8_t (*Cmd_BuildCommandFunc)(CMD_t *c); + +typedef struct { + CMD_InputSource_t source; + Cmd_BuildCommandFunc chassisFunc; + Cmd_BuildCommandFunc gimbalFunc; + Cmd_BuildCommandFunc shootFunc; +} CMD_SourceHandler_t; + +CMD_SourceHandler_t sourceHandlers[CMD_SRC_NUM] = { + {CMD_SRC_RC, Cmd_BuildChassisCommandFromInput_rc, Cmd_BuildGimbalCommandFromInput_rc, Cmd_BuildShootCommandFromInput_rc}, + {CMD_SRC_PC, Cmd_BuildChassisCommandFromInput_pc, Cmd_BuildGimbalCommandFromInput_pc, Cmd_BuildShootCommandFromInput_pc}, + {CMD_SRC_NUC, Cmd_BuildChassisCommandFromInput_nuc, Cmd_BuildGimbalCommandFromInput_nuc, Cmd_BuildShootCommandFromInput_nuc}, + {CMD_SRC_REFEREE, Cmd_BuildChassisCommandFromInput_referee, Cmd_BuildGimbalCommandFromInput_referee, Cmd_BuildShootCommandFromInput_referee}, +}; + +int8_t Cmd_GenerateCommand(CMD_t *c) { + if (c == NULL) { + return CMD_ERR_NULL; // 参数错误 + } + + Cmd_Arbiter(c); + switch (c->out_chassis.source) { + case CMD_SRC_RC: + sourceHandlers[CMD_SRC_RC].chassisFunc(c); + break; + case CMD_SRC_PC: + sourceHandlers[CMD_SRC_PC].chassisFunc(c); + break; + case CMD_SRC_NUC: + sourceHandlers[CMD_SRC_NUC].chassisFunc(c); + break; + case CMD_SRC_REFEREE: + sourceHandlers[CMD_SRC_REFEREE].chassisFunc(c); + break; + default: + break; + } + switch (c->out_gimbal.source) { + case CMD_SRC_RC: + sourceHandlers[CMD_SRC_RC].gimbalFunc(c); + break; + case CMD_SRC_PC: + sourceHandlers[CMD_SRC_PC].gimbalFunc(c); + break; + case CMD_SRC_NUC: + sourceHandlers[CMD_SRC_NUC].gimbalFunc(c); + break; + case CMD_SRC_REFEREE: + sourceHandlers[CMD_SRC_REFEREE].gimbalFunc(c); + break; + default: + break; + } + switch (c->out_shoot.source) { + case CMD_SRC_RC: + sourceHandlers[CMD_SRC_RC].shootFunc(c); + break; + case CMD_SRC_PC: + sourceHandlers[CMD_SRC_PC].shootFunc(c); + break; + case CMD_SRC_NUC: + sourceHandlers[CMD_SRC_NUC].shootFunc(c); + break; + case CMD_SRC_REFEREE: + sourceHandlers[CMD_SRC_REFEREE].shootFunc(c); + break; + default: + break; + } + + return CMD_OK; +} diff --git a/User/module/cmd.h b/User/module/cmd.h index 4002408..2b8b895 100644 --- a/User/module/cmd.h +++ b/User/module/cmd.h @@ -22,57 +22,193 @@ extern "C" { /* Exported macro ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */ -typedef union rc_u rc_u; - -typedef struct { - rc_u *rc; - enum { DR16, AT9S } rc_type; -} CMD_RCInputData_t; +typedef union rc_inputdata_u rc_inputdata_u; #define CMD_REFEREE_MAX_NUM (3) /* 发送命令限定的最大数量 */ /* 输入源枚举 */ typedef enum { - CMD_SRC_RC, /* 遥控器 */ + CMD_SRC_RC=0, /* 遥控器 */ CMD_SRC_PC, /* 键盘鼠标 */ CMD_SRC_NUC, /* 上位机 */ CMD_SRC_REFEREE, /* 裁判系统 */ - CMD_SRC_NUM + CMD_SRC_NUM, + CMD_SRC_ERR } CMD_InputSource_t; +/* RC part begin-------------------------------------- */ typedef struct { - -}CMD_PCInputData_t; + bool online; + enum {DR16, AT9S} type; + rc_inputdata_u *inputData; +} CMD_InputData_RC_t; + + +typedef struct { + +} CMD_RC_Param_t; + +typedef struct { + CMD_InputData_RC_t input; +} CMD_RC_t; +/* RC part end---------------------------------------- */ + +/* PC part begin-------------------------------------- */ +/* 键盘按键值 */ +typedef enum { + CMD_KEY_W = 0, + CMD_KEY_S, + CMD_KEY_A, + CMD_KEY_D, + CMD_KEY_SHIFT, + CMD_KEY_CTRL, + CMD_KEY_Q, + CMD_KEY_E, + CMD_KEY_R, + CMD_KEY_F, + CMD_KEY_G, + CMD_KEY_Z, + CMD_KEY_X, + CMD_KEY_C, + CMD_KEY_V, + CMD_KEY_B, + CMD_L_CLICK, + CMD_R_CLICK, + CMD_KEY_NUM, +} CMD_KeyValue_t; + +typedef enum { + CMD_ACTIVE_RISING_EDGE, /* 按下时触发 */ + CMD_ACTIVE_FALLING_EDGE, /* 抬起时触发 */ + CMD_ACTIVE_PRESSED, /* 按住时触发 */ +} CMD_TriggerType_t; + +/* 行为值序列 */ +typedef enum { + CMD_BEHAVIOR_FORE = 0, /* 向前 */ + CMD_BEHAVIOR_BACK, /* 向后 */ + CMD_BEHAVIOR_LEFT, /* 向左 */ + CMD_BEHAVIOR_RIGHT, /* 向右 */ + CMD_BEHAVIOR_ACCELERATE, /* 加速 */ + CMD_BEHAVIOR_DECELEBRATE, /* 减速 */ + CMD_BEHAVIOR_FIRE, /* 开火 */ + CMD_BEHAVIOR_FIRE_MODE, /* 切换开火模式 */ + CMD_BEHAVIOR_BUFF, /* 打符模式 */ + CMD_BEHAVIOR_AUTOAIM, /* 自瞄模式 */ + CMD_BEHAVIOR_OPENCOVER, /* 弹舱盖开关 */ + CMD_BEHAVIOR_ROTOR, /* 小陀螺模式 */ + CMD_BEHAVIOR_REVTRIG, /* 反转拨弹 */ + CMD_BEHAVIOR_FOLLOWGIMBAL35, /* 跟随云台呈35度 */ + CMD_BEHAVIOR_NUM, +} CMD_Behavior_t; +typedef struct { + CMD_KeyValue_t key; + CMD_TriggerType_t trigger_type; +} CMD_KeyMapItem_t; + +/* 行为映射的对应按键数组 */ +typedef struct { + CMD_KeyMapItem_t key_map[CMD_BEHAVIOR_NUM]; +} CMD_KeyMap_Params_t; + +typedef struct { + struct { + int16_t x; + int16_t y; + int16_t z; + bool l_click; /* 左键 */ + bool r_click; /* 右键 */ + } mouse; /* 鼠标值 */ + + uint16_t key; /* 按键值 */ + + uint16_t res; /* 保留，未启用 */ +} CMD_PC_data_t; + +typedef struct { + bool online; + CMD_PC_data_t data; +}CMD_InputData_PC_t; typedef struct { -}CMD_NUCInputData_t; +} CMD_PC_Param_t; + +typedef struct { + CMD_InputData_PC_t input; +} CMD_PC_t; +/* PC part end---------------------------------------- */ + +/* NUC part begin------------------------------------- */ +typedef struct { + bool online; +}CMD_InputData_NUC_t; typedef struct { -}CMD_REFInputData_t; +} CMD_NUC_Param_t; + +typedef struct { + CMD_InputData_NUC_t input; +} CMD_NUC_t; +/* NUC part end--------------------------------------- */ + +/* REF part begin------------------------------------- */ +typedef struct { + bool online; +}CMD_InputData_REF_t; + +typedef struct { + +} CMD_REF_Param_t; + +typedef struct { + CMD_InputData_REF_t input; +} CMD_REF_t; +/* REF part begin------------------------------------- */ /* 底盘控制命令 */ typedef struct { - -} CMD_cmdForChassisCtrl_t; + CMD_InputSource_t source; + Chassis_CMD_t cmd; +} CMD_OutputCmd_CHASSIS_t; /* 云台控制命令 */ typedef struct { - -} CMD_cmdForGimbalCtrl_t; + CMD_InputSource_t source; + Gimbal_CMD_t cmd; +} CMD_OutputCmd_GIMBAL_t; /* 射击控制命令 */ typedef struct { - CMD_InputSource_t src; - Shoot_CMD_t shoot_cmd; -} CMD_Shoot_t; + CMD_InputSource_t source; + Shoot_CMD_t cmd; +} CMD_OutputCmd_SHOOT_t; typedef struct { - bool pc_ctrl; /* 是否使用键鼠控制 */ - bool host_overwrite; /* 是否Host控制 */ + CMD_InputSource_t source; + uint8_t priority; +} CMD_SourcePriorityConfig_t; + +typedef struct { + CMD_SourcePriorityConfig_t SourcePriorityConfigs[CMD_SRC_NUM]; +} CMD_Param_t; + +typedef struct { + uint16_t key_last; /* 上次按键键值 */ + CMD_Param_t params; + + CMD_RC_t rc; + CMD_PC_t pc; + CMD_NUC_t nuc; + CMD_REF_t ref; + + CMD_OutputCmd_CHASSIS_t out_chassis; + CMD_OutputCmd_GIMBAL_t out_gimbal; + CMD_OutputCmd_SHOOT_t out_shoot; + } CMD_t; /* Exported functions prototypes -------------------------------------------- */ diff --git a/User/module/config.c b/User/module/config.c index a3bc6af..eae6f7c 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -16,6 +16,84 @@ // 机器人参数配置 Config_RobotParam_t robot_config = { + + .chassis_param = { + + .motor_param = { + { + .can = BSP_CAN_1, + .id = 0x201, + .module = MOTOR_M3508, + .reverse = false, + .gear = true + }, + { + .can = BSP_CAN_1, + .id = 0x202, + .module = MOTOR_M3508, + .reverse = false, + .gear = true + }, + { + .can = BSP_CAN_1, + .id = 0x203, + .module = MOTOR_M3508, + .reverse = false, + .gear = true + }, + { + .can = BSP_CAN_1, + .id = 0x204, + .module = MOTOR_M3508, + .reverse = false, + .gear = true + }, + }, + .type = CHASSIS_TYPE_OMNI_PLUS, + + .pid = { + + .motor_pid_param = { + .k = 0.001f, + .p = 0.01f, + .i = 0.01f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .range = -1.0f, + }, + + + .follow_pid_param = { + .k = 0.5f, + .p = 1.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .range = M_2PI, + }, + }, + + + .low_pass_cutoff_freq = { + .in = 50.0f, + .out = 50.0f, + }, + + + .reverse = { + .yaw = true, + }, + .limit = { + .max_vx = 3.0f, + .max_vy = 3.0f, + .max_wz = 2.0f, + .max_current = 16000.0f + }, + }, .gimbal_param = { diff --git a/User/module/config.h b/User/module/config.h index b09644a..2762208 100644 --- a/User/module/config.h +++ b/User/module/config.h @@ -18,7 +18,7 @@ extern "C" { typedef struct { Shoot_Params_t shoot_param; Gimbal_Params_t gimbal_param; -// Chassis_Params_t chassis_param; + Chassis_Params_t chassis_param; } Config_RobotParam_t; /* Exported functions prototypes -------------------------------------------- */ diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c index abe0d5f..3ca0093 100644 --- a/User/task/ctrl_chassis.c +++ b/User/task/ctrl_chassis.c @@ -4,11 +4,10 @@ */ /* Includes ----------------------------------------------------------------- */ -#include "task/user_task.h" + #include "task/user_task.h" /* USER INCLUDE BEGIN */ -#include #include "module/chassis.h" -#include "component/at9s_pro_cmd.h" +#include "module/config.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -16,8 +15,10 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ -chassis_ctrl_eulr_t chassis_ctrl_eulr; -COMP_AT9S_CMD_t chassis_ctrl_cmd_rc; +Chassis_t chassis; +Chassis_CMD_t chassis_cmd; +Chassis_IMU_t chassis_imu; +static AHRS_Eulr_t mech_zero = {0}; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -33,18 +34,20 @@ void Task_ctrl_chassis(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ -Motor_Init(CTRL_CHASSIS_FREQ); + Config_RobotParam_t *cfg = Config_GetRobotParam(); + Chassis_Init(&chassis, &cfg->chassis_param, (float)CTRL_CHASSIS_FREQ); + chassis.mech_zero=4.13f; /* USER CODE INIT END */ while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ -osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_ctrl_cmd_rc, NULL, 0); - osMessageQueueGet(task_runtime.msgq.chassis.yaw, &chassis_ctrl_eulr.chassis_encoder_yaw, NULL, 0); + osMessageQueueGet(task_runtime.msgq.chassis.yaw, &chassis.feedback.encoder_gimbalYawMotor, NULL, 0); + osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_cmd, NULL, 0);//遥控器 - chassis_ctrl_eulr.chassis_mech_zero_yaw=4.13f; -//// chassis_ctrl_eulr.chassis_encoder_yaw=motor_gimbal_yaw.total_angle; - chassis_control (chassis_ctrl_eulr,chassis_ctrl_cmd_rc); + Chassis_UpdateFeedback(&chassis); + Chassis_Control(&chassis, &chassis_cmd, osKernelGetTickCount()); + Chassis_Output(&chassis); /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } diff --git a/ozone/hero.jdebug b/ozone/hero.jdebug index 224a598..01d7b4b 100644 --- a/ozone/hero.jdebug +++ b/ozone/hero.jdebug @@ -5,7 +5,7 @@ ********************************************************************** File : -Created : 14. Oct 2025 21:13 +Created : 02. Nov 2025 13:12 Ozone Version : V3.40b */ @@ -22,8 +22,8 @@ void OnProjectLoad (void) { // // Dialog-generated settings // - Project.AddPathSubstitute ("D:/CUBEMX/hero/ozone", "$(ProjectDir)"); - Project.AddPathSubstitute ("d:/cubemx/hero/ozone", "$(ProjectDir)"); + Project.AddPathSubstitute ("D:/CUBEMX/hero/god-yuan-hero/ozone", "$(ProjectDir)"); + Project.AddPathSubstitute ("d:/cubemx/hero/god-yuan-hero/ozone", "$(ProjectDir)"); Project.SetDevice ("STM32F407IG"); Project.SetHostIF ("USB", ""); Project.SetTargetIF ("SWD"); @@ -32,7 +32,7 @@ void OnProjectLoad (void) { // // User settings // - File.Open ("D:/CUBEMX/hero/MDK-ARM/hero/hero.axf"); + File.Open ("D:/CUBEMX/hero/god-yuan-hero/build/Debug/hero.elf"); } /********************************************************************* diff --git a/ozone/hero.jdebug.user b/ozone/hero.jdebug.user index 2c08892..f3a7a9a 100644 --- a/ozone/hero.jdebug.user +++ b/ozone/hero.jdebug.user @@ -1,42 +1,26 @@ -GraphedExpression="((shoot).target_variable).target_rpm", Color=#e56a6f, Show=0 -GraphedExpression="(shoot).errtosee", Color=#35792b, Show=0 -GraphedExpression="chassis_out[2]", Color=#769dda -OpenDocument="startup_stm32f407xx.s", FilePath="D:/CUBEMX/hero/MDK-ARM/startup_stm32f407xx.s", Line=161 -OpenDocument="chassis.h", FilePath="D:/CUBEMX/hero/User/module/chassis.h", Line=0 -OpenDocument="ctrl_chassis.c", FilePath="D:/CUBEMX/hero/User/task/ctrl_chassis.c", Line=4 -OpenDocument="pid.h", FilePath="D:/CUBEMX/hero/User/component/pid.h", Line=0 -OpenDocument="shoot.h", FilePath="D:/CUBEMX/hero/User/module/shoot.h", Line=166 -OpenDocument="bmi088.c", FilePath="D:/CUBEMX/hero/User/device/bmi088.c", Line=159 -OpenDocument="stm32f4xx_it.c", FilePath="D:/CUBEMX/hero/Core/Src/stm32f4xx_it.c", Line=87 -OpenDocument="shoot.c", FilePath="D:/CUBEMX/hero/User/module/shoot.c", Line=364 -OpenDocument="ctrl_shoot.c", FilePath="D:/CUBEMX/hero/User/task/ctrl_shoot.c", Line=0 -OpenDocument="can.c", FilePath="D:/CUBEMX/hero/User/bsp/can.c", Line=485 -OpenDocument="ctrl_gimbal.c", FilePath="D:/CUBEMX/hero/User/task/ctrl_gimbal.c", Line=0 -OpenDocument="chassis.c", FilePath="D:/CUBEMX/hero/User/module/chassis.c", Line=20 -OpenDocument="main.c", FilePath="D:/CUBEMX/hero/Core/Src/main.c", Line=48 +OpenDocument="ctrl_shoot.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_shoot.c", Line=0 +OpenDocument="ctrl_gimbal.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_gimbal.c", Line=14 +OpenDocument="ctrl_chassis.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_chassis.c", Line=7 +OpenDocument="user_task.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/user_task.c", Line=0 +OpenDocument="main.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Core/Src/main.c", Line=60 OpenToolbar="Debug", Floating=0, x=0, y=0 -OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=726, h=609, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1 +OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=726, h=599, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1 OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=301, h=919, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=726, h=309, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=1, w=726, h=609, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=726, h=319, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=1, w=726, h=599, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=301, h=919, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Data Sampling", DockArea=BOTTOM, x=1, y=0, w=1279, h=536, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 -OpenWindow="Timeline", DockArea=BOTTOM, x=0, y=0, w=1280, h=555, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="1 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="758;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1053;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="1004;0" -OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=1279, h=536, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Data Sampling", DockArea=BOTTOM, x=1, y=0, w=1276, h=536, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 +OpenWindow="Timeline", DockArea=BOTTOM, x=0, y=0, w=1283, h=555, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=1, CodePaneShown=1, PinCursor="Cursor Movable", TimePerDiv="1 ns / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="1056;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=1, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1056;-69", CodeGraphLegendShown=1, CodeGraphLegendPosition="1007;0" +OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=1276, h=536, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1" -TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;144;482] TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1594;104;100;100;100] +TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1014] +TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100] +TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;100;100;100;100;110;126;422] TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100] +TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;144;294] +TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[250;282;91;103] TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[] -TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;726] -TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[250;282;91;100] -TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ((shoot).target_variable).target_rpm";" (shoot).errtosee";" chassis_out[2]"], ColWidths=[100;100;100;100;100] -TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;144;134;124;134;110;134;281] -TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;340] -WatchedExpression="gimbal", RefreshRate=5, Window=Watched Data 1 -WatchedExpression="shoot", RefreshRate=5, Window=Watched Data 1 -WatchedExpression="shoot.feedback.fric[2]", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="chassis_out", RefreshRate=5, Window=Watched Data 1 -WatchedExpression="shoot_cmd", RefreshRate=5, Window=Watched Data 1 \ No newline at end of file +TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;366] \ No newline at end of file