diff --git a/CMakeLists.txt b/CMakeLists.txt index 387e40c..f9262e4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,7 +89,6 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE modules/imu/ins_task.c # modules - communication modules/master_machine/master_machine.c - modules/remote/remote_control.c modules/timachine/board_comm.c # modules - motor modules/motor/DMmotor/dmmotor.c @@ -135,8 +134,6 @@ target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE modules/master_machine modules/motor/DMmotor modules/motor - modules/remote - modules/can_comm modules/message_center modules/daemon modules/alarm diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index c0a4f25..d7583b3 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -3,7 +3,6 @@ #include "robot_cmd.h" /* module */ #include "controller.h" -#include "remote_control.h" #include "ins_task.h" #include "message_center.h" #include "general_def.h" @@ -36,12 +35,6 @@ static Subscriber_t *gimbal_feed_sub; static Gimbal_Ctrl_Cmd_s gimbal_cmd_send; static Gimbal_Upload_Data_s gimbal_fetch_data; -/* ---- 遥控器 ---- */ -static RC_ctrl_t *rc_data; - -/* ---- 机器人状态 ---- */ -static Robot_Status_e robot_state; - /* ---- 激光 ---- */ static void laser_on(void) { @@ -56,7 +49,6 @@ static void laser_off(void) /* ---- 视觉 PID 和卡尔曼滤波器重置 ---- */ static void VisionPIDReset(void) { - /* 清零积分项和上次误差 */ vision_yaw_pid.Iout = 0; vision_yaw_pid.ITerm = 0; vision_yaw_pid.Last_Err = 0; @@ -75,7 +67,6 @@ static void VisionPIDReset(void) vision_pitch_pid.Output = 0; vision_pitch_pid.Last_Dout = 0; - /* 重置滤波器状态: x = [0, 0], P = diag(5, 5) */ memset(yaw_error_kf.xhat_data, 0, yaw_error_kf.xhatSize * sizeof(float)); memset(yaw_error_kf.P_data, 0, yaw_error_kf.xhatSize * yaw_error_kf.xhatSize * sizeof(float)); memset(pitch_error_kf.xhat_data, 0, pitch_error_kf.xhatSize * sizeof(float)); @@ -89,17 +80,16 @@ static void VisionPIDReset(void) filtered_pitch_error = 0; } -/* ---- 模式切换处理 ---- */ +/* ---- 模式切换 ---- */ static void SwitchToAutoMode(void) { if (current_mode == VISION_MODE_AUTO) return; - /* 同步角度参考值为当前 IMU 角度 */ gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data->YawTotalAngle; gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch; - vision_data.manual_updated = 0; // 清除手动数据残留 + vision_data.manual_updated = 0; VisionPIDReset(); current_mode = VISION_MODE_AUTO; LOGINFO("[CMD] switched to AUTO mode"); @@ -113,7 +103,7 @@ static void SwitchToManualMode(void) gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data->YawTotalAngle; gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch; - vision_data.auto_updated = 0; // 清除自动数据残留 + vision_data.auto_updated = 0; current_mode = VISION_MODE_MANUAL; LOGINFO("[CMD] switched to MANUAL mode"); } @@ -178,8 +168,6 @@ static void VisionKalmanFilterInit() void RobotCMDInit() { - rc_data = RemoteControlInit(&huart5); - gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); @@ -204,16 +192,12 @@ void RobotCMDInit() VisionKalmanFilterInit(); Vision_Uart_Init(&huart10); - - robot_state = ROBOT_READY; laser_off(); } /* ---- 自动 / 视觉追踪模式 ---- */ static void VisionTrackingSet() { - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - if (vision_data.auto_updated) { vision_data.auto_updated = 0; @@ -242,32 +226,15 @@ static void VisionTrackingSet() gimbal_cmd_send.pitch = gimbal_pitch_ref; } -/* ---- 手动模式 ---- */ +/* ---- 手动模式: 上位机角度参考 ---- */ static void ManualModeSet() { - if (vision_data.manual_updated && vision_data.zero_force) + if (vision_data.manual_updated) { - /* 零力子模式 */ vision_data.manual_updated = 0; - gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; - gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data->YawTotalAngle; - gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch; - } - else if (vision_data.manual_updated) - { - /* 上位机角度参考 */ - vision_data.manual_updated = 0; - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; gimbal_yaw_ref = vision_data.yaw_ref; gimbal_pitch_ref = vision_data.pitch_ref; } - else - { - /* 遥控器 */ - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - gimbal_yaw_ref -= 0.0025f * (float)rc_data[TEMP].rc.rocker_r_; - gimbal_pitch_ref += 0.00125f * (float)rc_data[TEMP].rc.rocker_r1; - } if (gimbal_pitch_ref > 30.0f) gimbal_pitch_ref = 30.0f; @@ -278,43 +245,12 @@ static void ManualModeSet() gimbal_cmd_send.pitch = gimbal_pitch_ref; } -/* ---- 急停 ---- */ -static void EmergencyHandler() -{ - if (switch_is_down(rc_data[TEMP].rc.switch_right)) - { - robot_state = ROBOT_STOP; - gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; - gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data->YawTotalAngle; - gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data->Pitch; - laser_off(); - } - - if (!switch_is_down(rc_data[TEMP].rc.switch_right) && robot_state == ROBOT_STOP) - { - robot_state = ROBOT_READY; - gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data->YawTotalAngle; - gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch; - LOGINFO("[CMD] reinstate, robot ready"); - } -} - void RobotCMDTask() { SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data); - /* 优先处理串口命令 (模式切换 / 激光等) */ ProcessCommand(); - EmergencyHandler(); - - if (robot_state == ROBOT_STOP) - { - PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send); - return; - } - - /* 根据当前模式执行控制 */ if (current_mode == VISION_MODE_AUTO) VisionTrackingSet(); else diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 2048fdf..5d9ddc8 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -84,17 +84,8 @@ void GimbalTask() { SubGetMessage(gimbal_sub, &gimbal_cmd_recv); - if (gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE) - { - /* 零力模式: 保持当前IMU角度, 电机不主动出力 */ - DMMotorSetRef(yaw_motor, gimbal_IMU_data->YawTotalAngle); - DMMotorSetRef(pitch_motor, gimbal_IMU_data->Pitch); - } - else - { - DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); - DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); - } + DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); + DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); gimbal_feedback_data.gimbal_imu_data = gimbal_IMU_data; PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); diff --git a/application/robot_def.h b/application/robot_def.h index a7fd019..aa7e508 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -24,13 +24,6 @@ #pragma pack(1) -/* 机器人状态 */ -typedef enum -{ - ROBOT_STOP = 0, - ROBOT_READY, -} Robot_Status_e; - /* 应用状态 */ typedef enum { @@ -39,19 +32,11 @@ typedef enum APP_ERROR, } App_Status_e; -/* 云台模式 */ -typedef enum -{ - GIMBAL_ZERO_FORCE = 0, - GIMBAL_GYRO_MODE, -} gimbal_mode_e; - /* ---- CMD 发布给 gimbal 的控制数据 ---- */ typedef struct { float yaw; // yaw 目标角度 (degrees, multi-turn) float pitch; // pitch 目标角度 (degrees) - gimbal_mode_e gimbal_mode; } Gimbal_Ctrl_Cmd_s; /* ---- gimbal 发布给 CMD 的反馈数据 ---- */ diff --git a/modules/master_machine/master_machine.h b/modules/master_machine/master_machine.h index 2c4fcb1..9a230d5 100644 --- a/modules/master_machine/master_machine.h +++ b/modules/master_machine/master_machine.h @@ -38,8 +38,8 @@ /* 命令码 */ #define VISION_CMD_SWITCH_AUTO 0x01 #define VISION_CMD_SWITCH_MANUAL 0x02 -#define VISION_CMD_LASER_ON 0x03 -#define VISION_CMD_LASER_OFF 0x04 +#define VISION_CMD_LASER_ON 0x03 +#define VISION_CMD_LASER_OFF 0x04 /* 工作模式 */ typedef enum