diff --git a/src/mc/src/mc.cpp b/src/mc/src/mc.cpp index 377d480..bd9af7c 100644 --- a/src/mc/src/mc.cpp +++ b/src/mc/src/mc.cpp @@ -229,9 +229,8 @@ void setupTimers(rclcpp::Node::SharedPtr node) vcu2_cmd.setSpray(msg.spray); vcu2_cmd.setSweeepCtrl(msg.sweep_ctrl); - // canctl.sendFrame(vcu1_cmd.toFrame()); - // canctl.sendFrame(vcu2_cmd.toFrame()); - }); + canctl.sendFrame(vcu1_cmd.toFrame()); + canctl.sendFrame(vcu2_cmd.toFrame()); }); } int main(int argc, char **argv) diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index c3017d7..b87bfe3 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/src/radio_ctrl/src/radio_ctrl.cpp @@ -8,7 +8,8 @@ namespace sweeperMsg = sweeper_interfaces::msg; -constexpr float EPS_GEAR_RATIO = 16.5f; +constexpr float DEG_PER_SEC_TO_RPM = 1.0f / 6.0f; +constexpr float GEAR_RATIO = 7.0f; constexpr float DELTA_T = 0.02f; // 20ms const uint16_t speed[3] = {192, 992, 1792}; @@ -90,17 +91,26 @@ private: { msg.gear = 1; // R挡 } - - // 油门 / 刹车逻辑 - if (ch_data[1] <= speed[1]) + else { + msg.gear = 0; msg.brake = 1; msg.rpm = 0; } - else + + if (ch_data[7] != 992) { - msg.brake = 0; - msg.rpm = static_cast(MCU_RPM_MAX * (ch_data[1] - speed[1]) / (speed[2] - speed[1])); + // 油门 / 刹车逻辑 + if (ch_data[1] <= speed[1]) + { + msg.brake = 1; + msg.rpm = 0; + } + else + { + msg.brake = 0; + msg.rpm = static_cast(MCU_RPM_MAX * (ch_data[1] - speed[1]) / (speed[2] - speed[1])); + } } // 一键清扫 @@ -122,13 +132,13 @@ private: } // 转向逻辑 - float target_angle = (992 - static_cast(ch_data[3])) / 800 * EPS_ANGLE_MAX; + float target_angle = (static_cast(ch_data[3]) - 992) / 800 * EPS_ANGLE_MAX; // 角速度(度/秒) float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T; // 电机转速(单位:rpm) - float motor_rpm = angle_speed * EPS_GEAR_RATIO; + float motor_rpm = angle_speed * DEG_PER_SEC_TO_RPM * GEAR_RATIO; // 限制电机转速到 [120, 1500] 范围,防止过小/过大 uint16_t can_speed = std::clamp(