From aa6085112c1d3a87e5a09347247853850bcdbdf1 Mon Sep 17 00:00:00 2001 From: cxh Date: Thu, 22 May 2025 08:51:18 +0800 Subject: [PATCH] temp0522 --- src/mc/src/mc.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/mc/src/mc.cpp b/src/mc/src/mc.cpp index 3a80390..8871f01 100644 --- a/src/mc/src/mc.cpp +++ b/src/mc/src/mc.cpp @@ -98,7 +98,8 @@ void setupTimers(rclcpp::Node::SharedPtr node) vcu1_cmd.setNightLightPercent(msg.night_light); canctl.sendFrame(mcu_cmd.toFrame()); - canctl.sendFrame(vcu1_cmd.toFrame()); }); + canctl.sendFrame(vcu1_cmd.toFrame()); + }); // EPS 控制,20Hz static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer( @@ -113,7 +114,8 @@ void setupTimers(rclcpp::Node::SharedPtr node) eps_cmd.setControlMode(0x20); eps_cmd.pack(); - canctl.sendFrame(eps_cmd.toFrame()); }); + canctl.sendFrame(eps_cmd.toFrame()); + }); // VCU2 控制,10Hz static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer( @@ -132,7 +134,8 @@ void setupTimers(rclcpp::Node::SharedPtr node) vcu2_cmd.setSpray(msg.spray); vcu2_cmd.setVacuum(msg.vacuum); - canctl.sendFrame(vcu2_cmd.toFrame()); }); + canctl.sendFrame(vcu2_cmd.toFrame()); + }); // 刷子控制,3.3Hz static rclcpp::TimerBase::SharedPtr timer_brush = node->create_wall_timer( @@ -148,7 +151,8 @@ void setupTimers(rclcpp::Node::SharedPtr node) edge_brush_cmd.setBrushPwm(msg.edge_brush_pwm); canctl.sendFrame(main_brush_cmd.toFrame()); - canctl.sendFrame(edge_brush_cmd.toFrame()); }); + canctl.sendFrame(edge_brush_cmd.toFrame()); + }); } void VCUWakeUp() @@ -198,7 +202,7 @@ int main(int argc, char **argv) mcCtrlCallback // 收到消息后的回调 ); - if (!canctl.open("can0")) + if (!canctl.open("can1")) { std::cerr << "Failed to open CAN interface\n"; return 1;