From ccfb03f3b76dd579d93532574b3573e7106859a4 Mon Sep 17 00:00:00 2001 From: TuxMonkey Date: Mon, 20 Apr 2026 15:01:50 +0800 Subject: [PATCH] add emghandler --- application/cmd/robot_cmd.c | 54 ++++++++++++++++++++----------------- 1 file changed, 29 insertions(+), 25 deletions(-) diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 59f39bb..4b7e3a2 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -153,27 +153,27 @@ static void RemoteControlSet() } // 右侧开关状态为[下],底盘旋转 - if (switch_is_down(rc_data[TEMP].rc.switch_right)) - { - chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; - - if (abs(rc_data[TEMP].rc.rocker_r_) > 300) - { - chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; - chassis_cmd_send.rotate_w = 2; - } - if (abs(rc_data[TEMP].rc.rocker_r1) > 300) - { - chassis_cmd_send.chassis_mode = CHASSIS_ROTATE_REVERSE; - chassis_cmd_send.rotate_w = -2; - } - chassis_cmd_send.vx = 0; - } - else - { + // if (switch_is_down(rc_data[TEMP].rc.switch_right)) + // { + // chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; + // + // if (abs(rc_data[TEMP].rc.rocker_r_) > 300) + // { + // chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; + // chassis_cmd_send.rotate_w = 2; + // } + // if (abs(rc_data[TEMP].rc.rocker_r1) > 300) + // { + // chassis_cmd_send.chassis_mode = CHASSIS_ROTATE_REVERSE; + // chassis_cmd_send.rotate_w = -2; + // } + // chassis_cmd_send.vx = 0; + // } + // else + // { chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; chassis_cmd_send.vx = 0.003f * (-(float)rc_data[TEMP].rc.rocker_r_ + (float)rc_data[TEMP].rc.rocker_r1); - } + // } // 左侧开关状态为[中]，腿长控制 if (switch_is_mid(rc_data[TEMP].rc.switch_left)) @@ -323,22 +323,26 @@ static void MouseKeySet() */ static void EmergencyHandler() { - if (switch_is_down(rc_data[TEMP].rc.switch_left) || switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制 + // 获取遥控器在线状态 (1:在线 0:离线) + uint8_t rc_online = RemoteControlIsOnline(); + + if (switch_is_down(rc_data[TEMP].rc.switch_left) || switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下]或[中] { - if ((rc_data[TEMP].rc.dial > 300 && switch_is_down(rc_data[TEMP].rc.switch_left)) || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断 + // 急停触发条件：遥控器离线 或者 右侧开关打到[下] 或者 robot_state 已经是 STOP + if (!rc_online || switch_is_down(rc_data[TEMP].rc.switch_right) || robot_state == ROBOT_STOP) { robot_state = ROBOT_STOP; gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; - chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; + chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; // 底盘电流归零，无力化 shoot_cmd_send.shoot_mode = SHOOT_OFF; shoot_cmd_send.friction_mode = FRICTION_OFF; shoot_cmd_send.load_mode = LOAD_STOP; - gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步，避免恢复时疯转 + gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步 gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; } - // 遥控器右侧开关为[上],恢复正常运行 - if (switch_is_up(rc_data[TEMP].rc.switch_right)) + // 恢复正常运行条件：遥控器在线 且 右侧开关状态为[上] + else if (switch_is_up(rc_data[TEMP].rc.switch_right) || switch_is_mid(rc_data[TEMP].rc.switch_right)) { robot_state = ROBOT_READY; shoot_cmd_send.shoot_mode = SHOOT_ON;