From 1af51b7737e8f1ab05df61fa612a09cd1ef556c5 Mon Sep 17 00:00:00 2001 From: gj Date: Tue, 9 Sep 2025 14:19:19 +0800 Subject: [PATCH] =?UTF-8?q?feat=20:=20=E9=81=A5=E6=8E=A7=E5=99=A8=E9=80=82?= =?UTF-8?q?=E9=85=8D=E6=96=B0=E5=BA=95=E7=9B=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/radio_ctrl/src/radio_ctrl.cpp | 40 +++++++++++++-------------- src/sweeper_interfaces/msg/McCtrl.msg | 2 +- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index cd54ead..a97fabf 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/src/radio_ctrl/src/radio_ctrl.cpp @@ -79,9 +79,9 @@ private: } // printf("\n"); - uint16_t gear = ch_data[5]; // 挡位 - uint16_t sweep = ch_data[6]; // 清扫 - int16_t speed = ch_data[1] - 992; //[-800,800] + uint16_t gear = ch_data[7]; // 挡位 最下1792，中间992，最上192 + uint16_t sweep = ch_data[6]; // 清扫 最上192，最下1792 + int16_t speed = ch_data[2] - 992; // 左边摇杆，最下192，中间992，最上1792 -> [-800,800] // 挡位选择 if (gear == 192) @@ -98,7 +98,7 @@ private: } // 油门 / 刹车逻辑 - if (speed <= 0) + if (speed < 0) { msg.brake = 1; msg.rpm = 0; @@ -106,10 +106,10 @@ private: else { msg.brake = 0; - msg.rpm = static_cast(std::clamp(MCU_RPM_MAX * speed / 800, 0, MCU_RPM_MAX)); + msg.rpm = static_cast(std::clamp(MCU_RPM_MAX * speed / 800, 0, MCU_RPM_MAX)); } - // 一键清扫 + // 不清扫 if (sweep == 1792) { msg.roll_brush_suction_direction = 0; @@ -120,6 +120,7 @@ private: msg.suction_squeeqee_pusher = 0; msg.water_spray = 0; } + // 清扫 else if (sweep == 192) { msg.roll_brush_suction_direction = 1; @@ -132,7 +133,7 @@ private: } // 转向逻辑 - float target_angle = (static_cast(ch_data[3]) - 992) / 800 * EPS_ANGLE_MAX; + float target_angle = (static_cast(ch_data[0]) - 947) / 800 * EPS_ANGLE_MAX; // 角速度（度/秒） float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T; @@ -152,20 +153,19 @@ private: // 发布控制消息 pub_->publish(msg); - std::cout << "Sub ControlMsg:" - << "\n 电磁刹: " << static_cast(msg.brake) - << "\n 挡位: " << static_cast(msg.gear) - << "\n 转速: " << static_cast(msg.rpm) - << "\n 轮端转向角度: " << msg.angle - << "\n 转向角速度: " << msg.angle_speed - << "\n 滚刷&吸风电机方向: " << static_cast(msg.roll_brush_suction_direction) - << "\n 滚刷&吸风电机: " << static_cast(msg.roll_brush_suction) - << "\n 边刷电机: " << static_cast(msg.side_brush) - << "\n 振尘电机: " << static_cast(msg.dust_shake) - << "\n 刷盘推杆电机: " << static_cast(msg.brush_deck_pusher) - << "\n 吸扒推杆电机: " << static_cast(msg.suction_squeeqee_pusher) - << "\n 喷水电机: " << static_cast(msg.water_spray) + std::cout << "\n 刹车: " << (msg.brake ? "已刹车" : "未刹车") + << "\n 挡位: "; + switch(msg.gear) { + case 0: std::cout << "空挡"; break; + case 1: std::cout << "前进挡"; break; + case 2: std::cout << "后退挡"; break; + default: std::cout << "未知挡位(" << static_cast(msg.gear) << ")"; break; + } + std::cout << "\n 行走电机转速: " << static_cast(msg.rpm) << " RPM" + << "\n 轮端转向角度: " << msg.angle << "°" + << "\n 清扫状态: " << (msg.roll_brush_suction_direction ? "正在清扫" : "未清扫") << std::endl; + } else { diff --git a/src/sweeper_interfaces/msg/McCtrl.msg b/src/sweeper_interfaces/msg/McCtrl.msg index 681b2ac..624aa1f 100644 --- a/src/sweeper_interfaces/msg/McCtrl.msg +++ b/src/sweeper_interfaces/msg/McCtrl.msg @@ -3,7 +3,7 @@ #mcu部分 uint8 brake #电磁刹指令 0开;1关 uint8 gear #挡位 0空挡;1前进;2后退;3保留 -uint8 rpm #转速 量程：0-6000，对应实际电机转速0-6000rpm +uint16 rpm #转速 量程：0-6000，对应实际电机转速0-6000rpm #eps部分 float32 angle #轮端转向角度 分辨率0.2° [-66.0,66.0] 适当缩减