diff --git a/CMakeLists.txt b/CMakeLists.txt index 3b8e9f0..6d7aefa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -57,7 +57,6 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE # User/component sources User/component/ahrs.c - User/component/cmd.c User/component/crc16.c User/component/crc8.c User/component/filter.c diff --git a/User/component/cmd.c b/User/component/cmd.c deleted file mode 100644 index 1effe69..0000000 --- a/User/component/cmd.c +++ /dev/null @@ -1,387 +0,0 @@ -/* - 控制命令 -*/ - -#include "cmd.h" - -#include - -/* USER INCLUDE BEGIN */ - -/* USER INCLUDE END */ - -/* USER DEFINE BEGIN */ - -/* USER DEFINE END */ - -/** - * @brief 行为转换为对应按键 - * - * @param cmd 主结构体 - * @param behavior 行为 - * @return uint16_t 行为对应的按键 - */ -static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd, - CMD_Behavior_t behavior) { - return cmd->param->map.key_map[behavior].key; -} - -static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd, - CMD_Behavior_t behavior) { - return cmd->param->map.key_map[behavior].active; -} - -/** - * @brief 检查按键是否按下 - * - * @param rc 遥控器数据 - * @param key 按键名称 - * @param stateful 是否为状态切换按键 - * @return true 按下 - * @return false 未按下 - */ -static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key) { - /* 按下按键为鼠标左、右键 */ - if (key == CMD_L_CLICK) { - return rc->mouse.l_click; - } - if (key == CMD_R_CLICK) { - return rc->mouse.r_click; - } - return rc->key & (1u << key); -} - -static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd, - CMD_Behavior_t behavior) { - CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior); - CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior); - - bool now_key_pressed, last_key_pressed; - - /* 按下按键为鼠标左、右键 */ - if (key == CMD_L_CLICK) { - now_key_pressed = rc->mouse.l_click; - last_key_pressed = cmd->mouse_last.l_click; - } else if (key == CMD_R_CLICK) { - now_key_pressed = rc->mouse.r_click; - last_key_pressed = cmd->mouse_last.r_click; - } else { - now_key_pressed = rc->key & (1u << key); - last_key_pressed = cmd->key_last & (1u << key); - } - - switch (active) { - case CMD_ACTIVE_PRESSING: - return now_key_pressed && !last_key_pressed; - case CMD_ACTIVE_RASING: - return !now_key_pressed && last_key_pressed; - case CMD_ACTIVE_PRESSED: - return now_key_pressed; - } -} - -/** - * @brief 解析pc行为逻辑 - * - * @param rc 遥控器数据 - * @param cmd 主结构体 - * @param dt_sec 两次解析的间隔 - */ -static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) { - cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; - - /* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */ - cmd->gimbal.delta_eulr.yaw = - (float)rc->mouse.x * dt_sec * cmd->param->sens_mouse; - cmd->gimbal.delta_eulr.pit = - (float)(-rc->mouse.y) * dt_sec * cmd->param->sens_mouse; - cmd->chassis.ctrl_vec.vx = cmd->chassis.ctrl_vec.vy = 0.0f; - cmd->shoot.reverse_trig = false; - - /* 按键行为映射相关逻辑 */ - if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE)) { - cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense; - } - if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK)) { - cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense; - } - if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT)) { - cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense; - } - if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT)) { - cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense; - } - if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE)) { - cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense; - cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense; - } - if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE)) { - cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense; - cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense; - } - if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE)) { - /* 切换至开火模式，设置相应的射击频率和弹丸初速度 */ - cmd->shoot.mode = SHOOT_MODE_LOADED; - cmd->shoot.fire = true; - } else { - /* 切换至准备模式，停止射击 */ - cmd->shoot.mode = SHOOT_MODE_LOADED; - cmd->shoot.fire = false; - } - if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE)) { - /* 每按一次依次切换开火下一个模式 */ - cmd->shoot.fire_mode++; - cmd->shoot.fire_mode %= FIRE_MODE_NUM; - } - if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR)) { - /* 切换到小陀螺模式 */ - cmd->chassis.mode = CHASSIS_MODE_ROTOR; - cmd->chassis.mode_rotor = ROTOR_MODE_RAND; - } - if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER)) { - /* 每按一次开、关弹舱盖 */ - cmd->shoot.cover_open = !cmd->shoot.cover_open; - } - if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) { - if (cmd->ai_status == AI_STATUS_HITSWITCH) { - /* 停止ai的打符模式，停用host控制 */ - CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP); - cmd->host_overwrite = false; - cmd->ai_status = AI_STATUS_STOP; - } else if (cmd->ai_status == AI_STATUS_AUTOAIM) { - /* 自瞄模式中切换失败提醒 */ - } else { - /* ai切换至打符模式，启用host控制 */ - CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START); - cmd->ai_status = AI_STATUS_HITSWITCH; - cmd->host_overwrite = true; - } - } - if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) { - if (cmd->ai_status == AI_STATUS_AUTOAIM) { - /* 停止ai的自瞄模式，停用host控制 */ - cmd->host_overwrite = false; - cmd->ai_status = AI_STATUS_STOP; - CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP); - } else { - /* ai切换至自瞄模式，启用host控制 */ - cmd->ai_status = AI_STATUS_AUTOAIM; - cmd->host_overwrite = true; - CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START); - } - } else { - cmd->host_overwrite = false; - // TODO: 修复逻辑 - } - if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) { - /* 按下拨弹反转 */ - cmd->shoot.reverse_trig = true; - } - if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) { - cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35; - } - /* 保存当前按下的键位状态 */ - cmd->key_last = rc->key; - memcpy(&(cmd->mouse_last), &(rc->mouse), sizeof(cmd->mouse_last)); -} - -/** - * @brief 解析rc行为逻辑 - * - * @param rc 遥控器数据 - * @param cmd 主结构体 - * @param dt_sec 两次解析的间隔 - */ -static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) { - switch (rc->sw_l) { - /* 左拨杆相应行为选择和解析 */ - case CMD_SW_UP: - cmd->chassis.mode = CHASSIS_MODE_BREAK; - break; - - case CMD_SW_MID: - cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL; - break; - - case CMD_SW_DOWN: - cmd->chassis.mode = CHASSIS_MODE_ROTOR; - cmd->chassis.mode_rotor = ROTOR_MODE_CW; - break; - - case CMD_SW_ERR: - cmd->chassis.mode = CHASSIS_MODE_RELAX; - break; - } - switch (rc->sw_r) { - /* 右拨杆相应行为选择和解析*/ - case CMD_SW_UP: - cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; - cmd->shoot.mode = SHOOT_MODE_SAFE; - break; - - case CMD_SW_MID: - cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; - cmd->shoot.fire = false; - cmd->shoot.mode = SHOOT_MODE_LOADED; - break; - - case CMD_SW_DOWN: - cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; - cmd->shoot.mode = SHOOT_MODE_LOADED; - cmd->shoot.fire_mode = FIRE_MODE_SINGLE; - cmd->shoot.fire = true; - break; - /* - case CMD_SW_UP: - cmd->gimbal.mode = GIMBAL_MODE_RELAX; - cmd->shoot.mode = SHOOT_MODE_SAFE; - break; - - case CMD_SW_MID: - cmd->gimbal.mode = GIMBAL_MODE_RELAX; - cmd->shoot.fire = false; - cmd->shoot.mode = SHOOT_MODE_LOADED; - break; - - case CMD_SW_DOWN: - cmd->gimbal.mode = GIMBAL_MODE_RELAX; - cmd->shoot.mode = SHOOT_MODE_LOADED; - cmd->shoot.fire_mode = FIRE_MODE_SINGLE; - cmd->shoot.fire = true; - break; - */ - case CMD_SW_ERR: - cmd->gimbal.mode = GIMBAL_MODE_RELAX; - cmd->shoot.mode = SHOOT_MODE_RELAX; - } - /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */ - cmd->chassis.ctrl_vec.vx = rc->ch_l_x; - cmd->chassis.ctrl_vec.vy = rc->ch_l_y; - cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc; - cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc; -} - -/** - * @brief rc失控时机器人恢复放松模式 - * - * @param cmd 主结构体 - */ -static void CMD_RcLostLogic(CMD_t *cmd) { - /* 机器人底盘、云台、射击运行模式恢复至放松模式 */ - cmd->chassis.mode = CHASSIS_MODE_RELAX; - cmd->gimbal.mode = GIMBAL_MODE_RELAX; - cmd->shoot.mode = SHOOT_MODE_RELAX; -} - -/** - * @brief 初始化命令解析 - * - * @param cmd 主结构体 - * @param param 参数 - * @return int8_t 0对应没有错误 - */ -int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param) { - /* 指针检测 */ - if (cmd == NULL) return -1; - if (param == NULL) return -1; - - /* 设置机器人的命令参数，初始化控制方式为rc控制 */ - cmd->pc_ctrl = false; - cmd->param = param; - - return 0; -} - -/** - * @brief 检查是否启用上位机控制指令覆盖 - * - * @param cmd 主结构体 - * @return true 启用 - * @return false 不启用 - */ -inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; } - -/** - * @brief 解析命令 - * - * @param rc 遥控器数据 - * @param cmd 命令 - * @param dt_sec 两次解析的间隔 - * @return int8_t 0对应没有错误 - */ -int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) { - /* 指针检测 */ - if (rc == NULL) return -1; - if (cmd == NULL) return -1; - - /* 在pc控制和rc控制间切换 */ - if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) && - CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_Q)) - cmd->pc_ctrl = true; - - if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) && - CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E)) - cmd->pc_ctrl = false; - /*c当rc丢控时，恢复机器人至默认状态 */ - if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) { - CMD_RcLostLogic(cmd); - } else { - if (cmd->pc_ctrl) { - CMD_PcLogic(rc, cmd, dt_sec); - } else { - CMD_RcLogic(rc, cmd, dt_sec); - } - } - return 0; -} - -/** - * @brief 解析上位机命令 - * - * @param host host数据 - * @param cmd 命令 - * @param dt_sec 两次解析的间隔 - * @return int8_t 0对应没有错误 - */ -int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) { - (void)dt_sec; /* 未使用dt_sec，消除警告 */ - /* 指针检测 */ - if (host == NULL) return -1; - if (cmd == NULL) return -1; - - /* 云台欧拉角设置为host相应的变化的欧拉角 */ - cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw; - cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit; - - /* host射击命令，设置不同的射击频率和弹丸初速度 */ - if (host->fire) { - cmd->shoot.mode = SHOOT_MODE_LOADED; - cmd->shoot.fire = true; - } else { - cmd->shoot.mode = SHOOT_MODE_SAFE; - } - return 0; -} - -/** - * @brief 添加向Referee发送的命令 - * - * @param ref 命令队列 - * @param cmd 要添加的命令 - * @return int8_t 0对应没有错误 - */ -int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) { - /* 指针检测 */ - if (ref == NULL) return -1; - /* 越界检测 */ - if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0) return -1; - - /* 添加机器人当前行为状态到画图的命令队列中 */ - ref->cmd[ref->counter] = cmd; - ref->counter++; - return 0; -} - -/* USER FUNCTION BEGIN */ - -/* USER FUNCTION END */ diff --git a/User/component/cmd.h b/User/component/cmd.h deleted file mode 100644 index df84538..0000000 --- a/User/component/cmd.h +++ /dev/null @@ -1,318 +0,0 @@ -/* - 控制命令 -*/ - -#pragma once - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include - -#include "component/ahrs.h" - -/* USER INCLUDE BEGIN */ - -/* USER INCLUDE END */ - -#define CMD_REFEREE_MAX_NUM (3) /* Lines 16 omitted */ - -/* USER DEFINE BEGIN */ - -/* USER DEFINE END */ - -/* 机器人型号 */ -typedef enum { - ROBOT_MODEL_INFANTRY = 0, /* 步兵机器人 */ - ROBOT_MODEL_HERO, /* 英雄机器人 */ - ROBOT_MODEL_ENGINEER, /* 工程机器人 */ - ROBOT_MODEL_DRONE, /* 空中机器人 */ - ROBOT_MODEL_SENTRY, /* 哨兵机器人 */ - ROBOT_MODEL_NUM, /* 型号数量 */ -} CMD_RobotModel_t; - -/* 底盘运行模式 */ -typedef enum { - CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ - CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */ - CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */ - CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 通过闭环控制使车头方向35度跟随云台 */ - CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */ - CHASSIS_MODE_INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */ - CHASSIS_MODE_OPEN, /* 开环模式。底盘运行不受PID控制，直接输出到电机 */ -} CMD_ChassisMode_t; - -/* 云台运行模式 */ -typedef enum { - GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ - GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ - GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */ -} CMD_GimbalMode_t; - -/* 射击运行模式 */ -typedef enum { - SHOOT_MODE_RELAX, /* 放松模式，电机不输出 */ - SHOOT_MODE_SAFE, /* 保险模式，电机闭环控制保持静止 */ - SHOOT_MODE_LOADED, /* 上膛模式，摩擦轮开启。随时准备开火 */ -} CMD_ShootMode_t; - -typedef enum { - FIRE_MODE_SINGLE, /* 单发开火模式 */ - FIRE_MODE_BURST, /* N连发开火模式 */ - FIRE_MODE_CONT, /* 持续开火模式 */ - FIRE_MODE_NUM, -} CMD_FireMode_t; - -/* 小陀螺转动模式 */ -typedef enum { - ROTOR_MODE_CW, /* 顺时针转动 */ - ROTOR_MODE_CCW, /* 逆时针转动 */ - ROTOR_MODE_RAND, /* 随机转动 */ -} CMD_RotorMode_t; - -/* 底盘控制命令 */ -typedef struct { - CMD_ChassisMode_t mode; /* 底盘运行模式 */ - CMD_RotorMode_t mode_rotor; /* 小陀螺转动模式 */ - MoveVector_t ctrl_vec; /* 底盘控制向量 */ -} CMD_ChassisCmd_t; - -/* 云台控制命令 */ -typedef struct { - CMD_GimbalMode_t mode; /* 云台运行模式 */ - AHRS_Eulr_t delta_eulr; /* 欧拉角变化角度 */ -} CMD_GimbalCmd_t; - -/* 射击控制命令 */ -typedef struct { - CMD_ShootMode_t mode; /* 射击运行模式 */ - CMD_FireMode_t fire_mode; /* 开火模式 */ - bool fire; /*开火*/ - bool cover_open; /* 弹舱盖开关 */ - bool reverse_trig; /* 拨弹电机状态 */ -} CMD_ShootCmd_t; - -/* 拨杆位置 */ -typedef enum { - CMD_SW_ERR = 0, - CMD_SW_UP = 1, - CMD_SW_MID = 3, - CMD_SW_DOWN = 2, -} CMD_SwitchPos_t; - -/* 键盘按键值 */ -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_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 enum { - CMD_ACTIVE_PRESSING, /* 按下时触发 */ - CMD_ACTIVE_RASING, /* 抬起时触发 */ - CMD_ACTIVE_PRESSED, /* 按住时触发 */ -} CMD_ActiveType_t; - -typedef struct { - CMD_ActiveType_t active; - CMD_KeyValue_t key; -} CMD_KeyMapItem_t; - -/* 行为映射的对应按键数组 */ -typedef struct { - CMD_KeyMapItem_t key_map[CMD_BEHAVIOR_NUM]; -} CMD_KeyMap_Params_t; - -/* 位移灵敏度参数 */ -typedef struct { - float move_sense; /* 移动灵敏度 */ - float move_fast_sense; /* 加速灵敏度 */ - float move_slow_sense; /* 减速灵敏度 */ -} CMD_Move_Params_t; - -typedef struct { - uint16_t width; - uint16_t height; -} CMD_Screen_t; - -/* 命令参数 */ -typedef struct { - float sens_mouse; /* 鼠标灵敏度 */ - float sens_rc; /* 遥控器摇杆灵敏度 */ - CMD_KeyMap_Params_t map; /* 按键映射行为命令 */ - CMD_Move_Params_t move; /* 位移灵敏度参数 */ - CMD_Screen_t screen; /* 屏幕分辨率参数 */ -} CMD_Params_t; - -/* AI行为状态 */ -typedef enum { - AI_STATUS_STOP, /* 停止状态 */ - AI_STATUS_AUTOAIM, /* 自瞄状态 */ - AI_STATUS_HITSWITCH, /* 打符状态 */ - AI_STATUS_AUTOMATIC /* 自动状态 */ -} CMD_AI_Status_t; - -/* UI所用行为状态 */ -typedef enum { - CMD_UI_NOTHING, /* 当前无状态 */ - CMD_UI_AUTO_AIM_START, /* 自瞄状态开启 */ - CMD_UI_AUTO_AIM_STOP, /* 自瞄状态关闭 */ - CMD_UI_HIT_SWITCH_START, /* 打符状态开启 */ - CMD_UI_HIT_SWITCH_STOP /* 打符状态关闭 */ -} CMD_UI_t; - -/*裁判系统发送的命令*/ -typedef struct { - CMD_UI_t cmd[CMD_REFEREE_MAX_NUM]; /* 命令数组 */ - uint8_t counter; /* 命令计数 */ -} CMD_RefereeCmd_t; - -typedef struct { - bool pc_ctrl; /* 是否使用键鼠控制 */ - bool host_overwrite; /* 是否Host控制 */ - uint16_t key_last; /* 上次按键键值 */ - - struct { - int16_t x; - int16_t y; - int16_t z; - bool l_click; /* 左键 */ - bool r_click; /* 右键 */ - } mouse_last; /* 鼠标值 */ - - CMD_AI_Status_t ai_status; /* AI状态 */ - - const CMD_Params_t *param; /* 命令参数 */ - - CMD_ChassisCmd_t chassis; /* 底盘控制命令 */ - CMD_GimbalCmd_t gimbal; /* 云台控制命令 */ - CMD_ShootCmd_t shoot; /* 射击控制命令 */ - CMD_RefereeCmd_t referee; /* 裁判系统发送命令 */ -} CMD_t; - -typedef struct { - float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */ - float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */ - float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */ - float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */ - - float ch_res; /* 第五通道值 */ - - CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */ - CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */ - - struct { - int16_t x; - int16_t y; - int16_t z; - bool l_click; /* 左键 */ - bool r_click; /* 右键 */ - } mouse; /* 鼠标值 */ - - uint16_t key; /* 按键值 */ - - uint16_t res; /* 保留，未启用 */ -} CMD_RC_t; - -typedef struct { - AHRS_Eulr_t gimbal_delta; /* 欧拉角的变化量 */ - - struct { - float vx; /* x轴移动速度 */ - float vy; /* y轴移动速度 */ - float wz; /* z轴转动速度 */ - } chassis_move_vec; /* 底盘移动向量 */ - - bool fire; /* 开火状态 */ -} CMD_Host_t; - -/** - * @brief 解析行为命令 - * - * @param rc 遥控器数据 - * @param cmd 主结构体 - */ -int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param); - -/** - * @brief 检查是否启用上位机控制指令覆盖 - * - * @param cmd 主结构体 - * @return true 启用 - * @return false 不启用 - */ -bool CMD_CheckHostOverwrite(CMD_t *cmd); - -/** - * @brief 解析命令 - * - * @param rc 遥控器数据 - * @param cmd 命令 - * @param dt_sec 两次解析的间隔 - * @return int8_t 0对应没有错误 - */ -int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec); - -/** - * @brief 解析上位机命令 - * - * @param host host数据 - * @param cmd 命令 - * @param dt_sec 两次解析的间隔 - * @return int8_t 0对应没有错误 - */ -int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec); - -/** - * @brief 添加向Referee发送的命令 - * - * @param ref 命令队列 - * @param cmd 要添加的命令 - * @return int8_t 0对应没有错误 - */ -int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd); - -/* USER FUNCTION BEGIN */ - -/* USER FUNCTION END */ - -#ifdef __cplusplus -} -#endif diff --git a/User/component/vmc.c b/User/component/vmc.c index 51f5396..f819c98 100644 --- a/User/component/vmc.c +++ b/User/component/vmc.c @@ -127,6 +127,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl // 计算角加速度 leg->dd_theta = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt); + VMC_GroundContactDetection(vmc); // 更新地面接触状态 return 0; } @@ -175,10 +176,10 @@ float VMC_GroundContactDetection(VMC_t *vmc) { // Fn = F0*cos(theta) + Tp*sin(theta)/L0 + mg leg->Fn = leg->F_virtual * cosf(leg->theta) + leg->T_virtual * sinf(leg->theta) / leg->L0 + - vmc->param.wheel_mass * 9.8f; // 添加轮子重力 + 10.0f; // 添加轮子重力，假设轮子质量约1kg // 地面接触判断 - leg->is_ground_contact = (leg->Fn > 10.0f); // 10N阈值 + leg->is_ground_contact = (leg->Fn > 20.0f); // 10N阈值 return leg->Fn; } diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 70cfcaa..fbd9577 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -71,9 +71,9 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { LowPassFilter2p_Reset(&c->filter.wheel_out[0], 0.0f); LowPassFilter2p_Reset(&c->filter.wheel_out[1], 0.0f); - c->mode = mode; c->state = 0; // 重置状态，确保每次切换模式时都重新初始化 + c->wz_multi=0.0f; return CHASSIS_OK; } @@ -255,6 +255,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { } break; case CHASSIS_MODE_WHELL_LEG_BALANCE: + case CHASSIS_MODE_ROTOR: // 执行LQR控制（包含PID腿长控制） Chassis_LQRControl(c, c_cmd); @@ -308,23 +309,24 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { // 目标速度设定（原始期望速度） float desired_velocity = c_cmd->move_vec.vx * 2; - + // 加速度限制处理 - float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x; + float velocity_change = + desired_velocity - c->chassis_state.last_target_velocity_x; float max_velocity_change = max_acceleration * c->dt; // 最大允许的速度变化 - + // 限制速度变化率（加速度限制） if (velocity_change > max_velocity_change) { velocity_change = max_velocity_change; } else if (velocity_change < -max_velocity_change) { velocity_change = -max_velocity_change; } - + // 应用加速度限制后的目标速度 target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change; c->chassis_state.target_velocity_x = target_dot_x; c->chassis_state.last_target_velocity_x = target_dot_x; - + // 更新目标位置 c->chassis_state.target_x += target_dot_x * c->dt; @@ -352,7 +354,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { LQR_State_t target_state = {0}; target_state.theta = 0.0f; // 目标摆杆角度 target_state.d_theta = 0.0f; // 目标摆杆角速度 - target_state.phi = -0.1f; // 目标俯仰角 + target_state.phi = -0.1f; // 目标俯仰角 target_state.d_phi = 0.0f; // 目标俯仰角速度 target_state.x = c->chassis_state.target_x; // 目标位置 target_state.d_x = target_dot_x; // 目标速度 @@ -361,14 +363,40 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { LQR_SetTargetState(&c->lqr[0], &target_state); LQR_SetTargetState(&c->lqr[1], &target_state); - LQR_Control(&c->lqr[0]); - LQR_Control(&c->lqr[1]); - + if (c->mode != CHASSIS_MODE_ROTOR){ c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle; - c->yaw_control.target_yaw = c->param->mech_zero_yaw; + c->yaw_control.target_yaw = + c->param->mech_zero_yaw + c_cmd->move_vec.vy * M_PI_2; c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt); + }else{ + CircleAdd(&c->wz_multi,0.000001, M_2PI); + c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle; + CircleAdd(&c->yaw_control.target_yaw,c->wz_multi, M_2PI); + c->yaw_control.yaw_force = + PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, + c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt); + } + + if (c->vmc_[0].leg.is_ground_contact) { + LQR_Control(&c->lqr[0]); + } else { + c->lqr[0].control_output.T = 0.0f; + c->lqr[0].control_output.Tp = + c->lqr[0].K_matrix[1][6] * (-c->vmc_[0].leg.theta + 0.0f) + + c->lqr[0].K_matrix[1][7] * (-c->vmc_[0].leg.d_theta + 0.0f); + c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制 + } + if (c->vmc_[1].leg.is_ground_contact) + LQR_Control(&c->lqr[1]); + else { + c->lqr[1].control_output.T = 0.0f; + c->lqr[1].control_output.Tp = + c->lqr[1].K_matrix[1][6] * (-c->vmc_[1].leg.theta + 0.0f) + + c->lqr[1].K_matrix[1][7] * (-c->vmc_[1].leg.d_theta + 0.0f); + c->yaw_control.yaw_force = 0.0f; // 单腿离地时关闭偏航控制 + } /* 轮毂力矩输出（参考C++版本的减速比） */ c->output.wheel[0] = @@ -422,15 +450,14 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { // 横滚角补偿力（左腿减少支撑力） virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + - pid_output + 60.0f + roll_compensation_force; + pid_output + 55.0f + roll_compensation_force; // VMC逆解算（包含摆角补偿） if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0], c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) { - // if (VMC_InverseSolve(&c->vmc_[0], 0.0f, - // Delta_Tp[0]) == 0) { VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]); + } else { // VMC失败，设为0力矩 c->output.joint[0] = 0.0f; @@ -448,13 +475,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { // 横滚角补偿力（右腿增加支撑力） virtual_force[1] = (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + - pid_output + 65.0f - roll_compensation_force; + pid_output + 60.0f - roll_compensation_force; // VMC逆解算（包含摆角补偿） if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1], c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) { - // if (VMC_InverseSolve(&c->vmc_[1], 0.0f, - // Delta_Tp[1]) == 0) { VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]); } else { diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index ad73733..c304bae 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -40,8 +40,16 @@ typedef enum { CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ CHASSIS_MODE_RECOVER, /* 复位模式 */ CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */ + CHASSIS_MODE_ROTOR, /* 小陀螺模式，底盘高速旋转 */ } Chassis_Mode_t; +typedef enum { + CHASSIS_JUMP_STATE_READY, /* 准备跳跃 */ + CHASSIS_JUMP_STATE_JUMPING, /* 跳跃中 */ + CHASSIS_JUMP_STATE_LANDING, /* 着陆中 */ + CHASSIS_JUMP_STATE_END, /* 跳跃结束 */ +} Chassis_JumpState_t; + typedef struct { Chassis_Mode_t mode; /* 底盘模式 */ MoveVector_t move_vec; /* 底盘运动向量 */ diff --git a/User/module/config.c b/User/module/config.c index ebdd892..4084d77 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -254,18 +254,18 @@ Config_RobotParam_t robot_config = { } }, .lqr_gains = { - .k11_coeff = { -1.508572585852218e+02f, 1.764949368139731e+02f, -9.850368126414553e+01f, -1.786139736968997e+00f }, // theta - .k12_coeff = { 6.466280284100411e+00f, -6.582699834130516e+00f, -7.131858380770703e+00f, -2.414590752579311e-01f }, // d_theta - .k13_coeff = { -7.182568574598301e+01f, 7.405968297046749e+01f, -2.690651220502175e+01f, -1.029850390463813e-01f }, // x - .k14_coeff = { -7.645548919162933e+01f, 7.988837668034076e+01f, -3.105733981791483e+01f, -4.296847184537235e-01f }, // d_x - .k15_coeff = { -9.403058188674812e+00f, 2.314767704216332e+01f, -1.651965553704901e+01f, 3.907902082528794e+00f }, // phi - .k16_coeff = { -4.023111056381187e+00f, 6.154951770170482e+00f, -3.705537084098432e+00f, 8.264904615264155e-01f }, // d_phi - .k21_coeff = { 1.254775776629822e+02f, -8.971732439896309e+01f, 4.744038360386896e+00f, 1.088353622361175e+01f }, // theta - .k22_coeff = { 1.061139172689013e+01f, -1.011235824540459e+01f, 3.034478775177782e+00f, 1.254920921163464e+00f }, // d_theta - .k23_coeff = { -2.675556963667067e+01f, 4.511381902505539e+01f, -2.800741296230217e+01f, 7.647205920058282e+00f }, // x - .k24_coeff = { -4.067721095665326e+01f, 6.068996620706580e+01f, -3.488384556019462e+01f, 9.374136714284193e+00f }, // d_x - .k25_coeff = { 7.359316334738203e+01f, -7.896223244854855e+01f, 2.961892943386949e+01f, 4.406632136721399e+00f }, // phi - .k26_coeff = { 1.624843000878248e+01f, -1.680831323767654e+01f, 6.018754311678180e+00f, 2.337719500754984e-01f }, // d_phi + .k11_coeff = { -1.664671105856041e+02f, 1.992942584227816e+02f, -1.128817545552375e+02f, -3.940642943456036e+00f }, // theta + .k12_coeff = { 6.755144599983973e+00f, -6.854975158723420e+00f, -7.879639603878262e+00f, -3.430989798548029e-01f }, // d_theta + .k13_coeff = { -3.418833492635837e+01f, 3.794539021170885e+01f, -1.520710246171202e+01f, -1.850485591688714e+00f }, // x + .k14_coeff = { -2.922101309139124e+01f, 3.441555363812500e+01f, -1.622627738805500e+01f, -2.575952007718017e+00f }, // d_x + .k15_coeff = { -2.999200539123322e+01f, 4.450774352569402e+01f, -2.617910446060207e+01f, 7.659044227304066e+00f }, // phi + .k16_coeff = { -6.929749592092386e+00f, 8.873396045817064e+00f, -4.631802944769120e+00f, 1.362572135878896e+00f }, // d_phi + .k21_coeff = { 1.839005962436392e+02f, -1.553482828633151e+02f, 3.110973707035321e+01f, 1.011416512244285e+01f }, // theta + .k22_coeff = { 8.875841429383772e+00f, -9.417579122119678e+00f, 3.419383245219720e+00f, 1.210428310841859e+00f }, // d_theta + .k23_coeff = { -1.702557573110695e+01f, 3.268593582331833e+01f, -2.244761887504000e+01f, 6.890533147883790e+00f }, // x + .k24_coeff = { -3.039626115318488e+01f, 4.694731664073340e+01f, -2.839718813188056e+01f, 8.384942967731277e+00f }, // d_x + .k25_coeff = { 1.199164883293124e+02f, -1.297942949573494e+02f, 5.072648972704373e+01f, 3.419795826934867e+00f }, // phi + .k26_coeff = { 1.995972391472551e+01f, -2.168740968259558e+01f, 8.589314159209088e+00f, 1.523026014539914e-01f }, // d_phi }, .virtual_chassis_param = { diff --git a/User/module/shoot.c b/User/module/shoot.c index 5c51976..064ca11 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -133,7 +133,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq) memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu)); memset(&s->output,0,sizeof(s->output)); - s->target_variable.target_rpm=4000.0f; + s->target_variable.target_rpm=200.0f; return 0; } diff --git a/utils/Simulation-master/balance/series_legs/get_k_length.m b/utils/Simulation-master/balance/series_legs/get_k_length.m index 98c489f..6dd903f 100644 --- a/utils/Simulation-master/balance/series_legs/get_k_length.m +++ b/utils/Simulation-master/balance/series_legs/get_k_length.m @@ -17,10 +17,10 @@ function K = get_k_length(leg_length) R1=0.072; %驱动轮半径 L1=leg_length/2; %摆杆重心到驱动轮轴距离 LM1=leg_length/2; %摆杆重心到其转轴距离 - l1=-0.03; %机体质心距离转轴距离 + l1=0.01; %机体质心距离转轴距离 mw1=0.60; %驱动轮质量 mp1=1.8; %杆质量 - M1=10.0; %机体质量 + M1=12.0; %机体质量 Iw1=mw1*R1^2; %驱动轮转动惯量 Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量 IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量 @@ -48,8 +48,8 @@ function K = get_k_length(leg_length) B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]); B=double(B); - Q=diag([2000 1 600 200 2000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 - R=[240 0;0 50]; %T Tp + Q=diag([800 100 2500 1500 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 + R=[140 0;0 50]; %T Tp K=lqr(A,B,Q,R);