From ba3b8da1779b96d82484d6eb86363166c4de67b5 Mon Sep 17 00:00:00 2001
From: root <root@Joker>
Date: Tue, 20 May 2025 10:28:03 +0800
Subject: [PATCH] temp

---
 src/mc/src/mc.cpp | 134 +++++++++++++++++++++++++++++++++-------------
 1 file changed, 96 insertions(+), 38 deletions(-)

diff --git a/src/mc/src/mc.cpp b/src/mc/src/mc.cpp
index 0e80b6c..3a80390 100644
--- a/src/mc/src/mc.cpp
+++ b/src/mc/src/mc.cpp
@@ -9,6 +9,31 @@
 
 CANDriver canctl;
 
+struct ControlCache
+{
+  std::mutex mutex; // 防止多线程冲突
+  mc::msg::McCtrl latest_msg;
+  bool has_data = false;
+
+  void update(const mc::msg::McCtrl &msg)
+  {
+    std::lock_guard<std::mutex> lock(mutex);
+    latest_msg = msg;
+    has_data = true;
+  }
+
+  bool get(mc::msg::McCtrl &msg)
+  {
+    std::lock_guard<std::mutex> lock(mutex);
+    if (!has_data)
+      return false;
+    msg = latest_msg;
+    return true;
+  }
+};
+
+ControlCache control_cache;
+
 void receiveHandler(const CANFrame &frame, void *userData)
 {
   // 获取 ROS 2 Publisher
@@ -50,50 +75,80 @@ void receiveHandler(const CANFrame &frame, void *userData)
 
 void mcCtrlCallback(const mc::msg::McCtrl::SharedPtr msg)
 {
-  mcu_cmd.setEnabled(msg->mcu_enabled);
-  mcu_cmd.setGear(msg->gear);
-  mcu_cmd.setRPM(msg->rpm);
-  mcu_cmd.setBrakeTime(msg->brake_time_ms);
-  mcu_cmd.setBrake(msg->brake);
-  mcu_cmd.pack();
+  control_cache.update(*msg);
+}
 
-  eps_cmd.setCenterCmd(0);
-  eps_cmd.setAngle(static_cast<int16_t>(msg->angle * 5));
-  eps_cmd.setAngularSpeed(msg->angle_speed);
-  eps_cmd.setControlMode(0x20);
-  eps_cmd.pack();
+void setupTimers(rclcpp::Node::SharedPtr node)
+{
+  // MCU、VCU1 控制，50Hz
+  static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer(
+      std::chrono::milliseconds(20), []()
+      {
+        mc::msg::McCtrl msg;
+        if (!control_cache.get(msg)) return;
 
-  vcu1_cmd.setLeftLight(msg->left_light);
-  vcu1_cmd.setNightLightPercent(msg->night_light);
+        mcu_cmd.setEnabled(msg.mcu_enabled);
+        mcu_cmd.setGear(msg.gear);
+        mcu_cmd.setRPM(msg.rpm);
+        mcu_cmd.setBrakeTime(msg.brake_time_ms);
+        mcu_cmd.setBrake(msg.brake);
+        mcu_cmd.pack();
 
-  vcu2_cmd.setBrakeLight(msg->brake_light);
-  vcu2_cmd.setDustShake(msg->dust_shake);
-  vcu2_cmd.setEdgeBrushLift(msg->edge_brush_lift);
-  vcu2_cmd.setHeadlight(msg->headlight);
-  vcu2_cmd.setMainBrushLift(msg->main_brush_lift);
-  vcu2_cmd.setMudFlap(msg->mud_flap);
-  vcu2_cmd.setRightLight(msg->right_light);
-  vcu2_cmd.setSpray(msg->spray);
-  vcu2_cmd.setVacuum(msg->vacuum);
+        vcu1_cmd.setLeftLight(msg.left_light);
+        vcu1_cmd.setNightLightPercent(msg.night_light);
 
-  main_brush_cmd.setBrushEnable(msg->main_brush_spin);
-  main_brush_cmd.setBrushPwm(msg->main_brush_pwm);
+        canctl.sendFrame(mcu_cmd.toFrame());
+        canctl.sendFrame(vcu1_cmd.toFrame()); });
 
-  edge_brush_cmd.setBrushEnable(msg->edge_brush_spin);
-  edge_brush_cmd.setBrushPwm(msg->edge_brush_pwm);
+  // EPS 控制，20Hz
+  static rclcpp::TimerBase::SharedPtr timer_eps = node->create_wall_timer(
+      std::chrono::milliseconds(50), []()
+      {
+        mc::msg::McCtrl msg;
+        if (!control_cache.get(msg)) return;
 
-  CANFrame mcu_frame = mcu_cmd.toFrame();
-  CANFrame eps_frame = eps_cmd.toFrame();
-  CANFrame vcu1_frame = vcu1_cmd.toFrame();
-  CANFrame vcu2_frame = vcu2_cmd.toFrame();
-  CANFrame main_brush_frame = main_brush_cmd.toFrame();
-  CANFrame edge_brush_frame = edge_brush_cmd.toFrame();
-  canctl.sendFrame(mcu_frame);
-  canctl.sendFrame(eps_frame);
-  canctl.sendFrame(vcu1_frame);
-  canctl.sendFrame(vcu2_frame);
-  canctl.sendFrame(main_brush_frame);
-  canctl.sendFrame(edge_brush_frame);
+        eps_cmd.setCenterCmd(0);
+        eps_cmd.setAngle(static_cast<int16_t>(msg.angle * 5));
+        eps_cmd.setAngularSpeed(msg.angle_speed);
+        eps_cmd.setControlMode(0x20);
+        eps_cmd.pack();
+
+        canctl.sendFrame(eps_cmd.toFrame()); });
+
+  // VCU2 控制，10Hz
+  static rclcpp::TimerBase::SharedPtr timer_vcu = node->create_wall_timer(
+      std::chrono::milliseconds(100), []()
+      {
+        mc::msg::McCtrl msg;
+        if (!control_cache.get(msg)) return;
+
+        vcu2_cmd.setBrakeLight(msg.brake_light);
+        vcu2_cmd.setDustShake(msg.dust_shake);
+        vcu2_cmd.setEdgeBrushLift(msg.edge_brush_lift);
+        vcu2_cmd.setHeadlight(msg.headlight);
+        vcu2_cmd.setMainBrushLift(msg.main_brush_lift);
+        vcu2_cmd.setMudFlap(msg.mud_flap);
+        vcu2_cmd.setRightLight(msg.right_light);
+        vcu2_cmd.setSpray(msg.spray);
+        vcu2_cmd.setVacuum(msg.vacuum);
+
+        canctl.sendFrame(vcu2_cmd.toFrame()); });
+
+  // 刷子控制，3.3Hz
+  static rclcpp::TimerBase::SharedPtr timer_brush = node->create_wall_timer(
+      std::chrono::milliseconds(300), []()
+      {
+        mc::msg::McCtrl msg;
+        if (!control_cache.get(msg)) return;
+
+        main_brush_cmd.setBrushEnable(msg.main_brush_spin);
+        main_brush_cmd.setBrushPwm(msg.main_brush_pwm);
+
+        edge_brush_cmd.setBrushEnable(msg.edge_brush_spin);
+        edge_brush_cmd.setBrushPwm(msg.edge_brush_pwm);
+
+        canctl.sendFrame(main_brush_cmd.toFrame());
+        canctl.sendFrame(edge_brush_cmd.toFrame()); });
 }
 
 void VCUWakeUp()
@@ -154,6 +209,9 @@ int main(int argc, char **argv)
   // 设置接收回调并传递 ROS 2 Publisher 作为 userData
   canctl.setReceiveCallback(receiveHandler, (void *)&publisher);
 
+  // 添加定时器设置
+  setupTimers(node);
+
   // ROS 2 spin
   rclcpp::spin(node);