From ab26100687c42fd75aa52cb926fdd093c479d57c Mon Sep 17 00:00:00 2001 From: cxh Date: Mon, 26 May 2025 18:05:46 +0800 Subject: [PATCH] auto --- a.sh | 6 ++++++ src/mc/config/config.json | 2 +- src/mc/src/mc.cpp | 20 ++++++++++++++------ src/radio_ctrl/src/radio_ctrl.cpp | 2 +- 4 files changed, 22 insertions(+), 8 deletions(-) create mode 100755 a.sh diff --git a/a.sh b/a.sh new file mode 100755 index 0000000..eb57d95 --- /dev/null +++ b/a.sh @@ -0,0 +1,6 @@ +sudo ip link set can0 down +sudo ip link set can0 up type can bitrate 500000 +sudo chmod 666 /dev/ttyUSB5 +sudo chmod 666 /dev/ttyUSB5 +source install/setup.bash +#sudo insmod drivers/usb/serial/pl2303.ko diff --git a/src/mc/config/config.json b/src/mc/config/config.json index cff7bee..0f5a1b7 100644 --- a/src/mc/config/config.json +++ b/src/mc/config/config.json @@ -1,3 +1,3 @@ { "can_dev": "can0" -} \ No newline at end of file +} diff --git a/src/mc/src/mc.cpp b/src/mc/src/mc.cpp index eae243a..ff4c362 100644 --- a/src/mc/src/mc.cpp +++ b/src/mc/src/mc.cpp @@ -95,11 +95,11 @@ void setupTimers(rclcpp::Node::SharedPtr node) mcu_cmd.setBrake(msg.brake); mcu_cmd.pack(); - vcu1_cmd.setLeftLight(msg.left_light); - vcu1_cmd.setNightLightPercent(msg.night_light); + canctl.sendFrame(mcu_cmd.toFrame()); - canctl.sendFrame(vcu1_cmd.toFrame()); }); + + }); // EPS 控制,20Hz static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer( @@ -114,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( @@ -123,6 +124,9 @@ void setupTimers(rclcpp::Node::SharedPtr node) mc::msg::McCtrl msg; if (!control_cache.get(msg)) return; + vcu1_cmd.setLeftLight(msg.left_light); + vcu1_cmd.setNightLightPercent(msg.night_light); + vcu2_cmd.setBrakeLight(msg.brake_light); vcu2_cmd.setDustShake(msg.dust_shake); vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift); @@ -133,7 +137,10 @@ void setupTimers(rclcpp::Node::SharedPtr node) vcu2_cmd.setSpray(msg.spray); vcu2_cmd.setVacuum(msg.vacuum); - canctl.sendFrame(vcu2_cmd.toFrame()); }); + //canctl.sendFrame(vcu1_cmd.toFrame()); + + canctl.sendFrame(vcu2_cmd.toFrame()); + }); // 刷子控制,3.3Hz static rclcpp::TimerBase::SharedPtr timer_brush = node->create_wall_timer( @@ -149,7 +156,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() diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index a23addb..89cea5d 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/src/radio_ctrl/src/radio_ctrl.cpp @@ -127,7 +127,7 @@ private: msg.vacuum = true; msg.spray = true; msg.mud_flap = true; - msg.dust_shake = true; + msg.dust_shake = false; msg.main_brush_spin = true; msg.edge_brush_spin = true; msg.main_brush_pwm = 100;