Auto commit at 2025-06-18 11:20:59

src/mc/include/mc/can_struct.h

@@ -57,6 +57,19 @@ union can_EPS_cmd_union
 
 #pragma pack(pop)
 
+union can_DBS_cmd_union
+{
+    struct __attribute__((packed))
+    {
+        uint8_t valid;       // Byte 0: VCU_DBS_Valid
+        uint8_t work_mode;   // Byte 1: VCU_DBS_Work_Mode
+        uint8_t pressure;    // Byte 2: VCU_DBS_HP_Pressure（0.1MPa）
+        uint8_t abs_active;  // Byte 3: ABS_Active
+        uint8_t reserved[4]; // Byte 4~7: 保留
+    } fields;
+    uint8_t bytes[8]; // 原始字节流
+};
+
 struct can_MCU_cmd
 {
     can_MCU_cmd_union data;
@@ -326,9 +339,52 @@ struct can_VCU_out2_cmd
     }
 };
 
+struct can_DBS_cmd
+{
+    can_DBS_cmd_union data;
+
+    static constexpr uint32_t CMD_ID = 0x154;
+    static constexpr bool EXT_FLAG = false;
+    static constexpr bool RTR_FLAG = false;
+
+    // 构造函数初始化固定字段
+    can_DBS_cmd()
+    {
+        memset(&data, 0, sizeof(data));
+        data.fields.work_mode = 0;
+        data.fields.abs_active = 1;
+    }
+
+    void setEnabled(bool en)
+    {
+        data.fields.work_mode = en ? 1 : 0;
+    }
+
+    // 设置压力（单位 MPa，0~8.0，有效范围 0~80）
+    can_DBS_cmd &setPressure(float mpa)
+    {
+        uint8_t val = static_cast<uint8_t>(std::clamp(mpa * 10.0f, 0.0f, 80.0f));
+        data.fields.pressure = val;
+        return *this;
+    }
+
+    // 转换为 CAN 帧
+    CANFrame toFrame() const
+    {
+        CANFrame frame;
+        frame.id = CMD_ID;
+        memcpy(frame.data, data.bytes, 8);
+        frame.dlc = 8;
+        frame.ext = EXT_FLAG;
+        frame.rtr = RTR_FLAG;
+        return frame;
+    }
+};
+
 extern can_MCU_cmd mcu_cmd;
 extern can_EPS_cmd eps_cmd;
 extern can_VCU_out1_cmd vcu1_cmd;
 extern can_VCU_out2_cmd vcu2_cmd;
+extern can_DBS_cmd dbs_cmd;
 
 #endif

src/mc/include/mc/dumper.h

@@ -0,0 +1,60 @@
+// 专门用于垃圾倾倒的头文件
+#ifndef DUMPER_H
+#define DUMPER_H
+
+#include "can_driver.h"
+#include <mutex>
+#include <queue>
+
+enum class DumperState
+{
+    IDLE,
+    RAISING,    // 斗升中
+    DUMPING,    // 放斗中
+    RETRACTING, // 收斗中
+    LOWERING    // 斗降中
+};
+
+enum class DumperAction
+{
+    RAISE,
+    DUMP,
+    RETRACT,
+    LOWER
+};
+
+class DumperController
+{
+public:
+    DumperController(CANDriver &driver);
+
+    void onStartup();                                 // 上电复位（只在内部用）
+    void tryStartup();                                // 唤醒后自动执行复位（防重）
+    void updateDial(int new_dial, int current_state); // 拨钮变化检测与处理
+    void resetAwake();                                // 清除唤醒状态（VCU 掉线）
+
+    int getSimpleState() const;
+
+private:
+    void handleDialChange(int from, int to);
+    void runActionSequence(const std::vector<DumperAction> &actions);
+    void doAction(DumperAction act);
+    void sendControlOff();
+    void logState() const;
+
+    bool raised_ = true;
+    bool dumped_ = true;
+    bool in_startup_reset_ = true;
+
+    bool awakened_ = false;
+    int last_dial_target_ = -1;
+
+    std::mutex queue_mutex_;
+    std::queue<DumperAction> action_queue_;
+    std::atomic<bool> busy_;
+    std::atomic<bool> has_pending_task_;
+    DumperState current_state_;
+    CANDriver &can_;
+};
+
+#endif

src/mc/src/can_struct.cpp

@@ -4,3 +4,4 @@ can_MCU_cmd mcu_cmd;
 can_EPS_cmd eps_cmd;
 can_VCU_out1_cmd vcu1_cmd;
 can_VCU_out2_cmd vcu2_cmd;
+can_DBS_cmd dbs_cmd;

src/mc/src/dumper.cpp

@@ -0,0 +1,211 @@
+#include "mc/dumper.h"
+#include <thread>
+#include <chrono>
+#include <iostream>
+
+DumperController::DumperController(CANDriver &driver)
+    : busy_(false),
+      has_pending_task_(false),
+      current_state_(DumperState::IDLE),
+      can_(driver)
+{
+}
+
+// 上电复位：自动收斗 + 降斗
+void DumperController::onStartup()
+{
+    std::cout << "[Init] Power-on reset: retract and lower...\n";
+    doAction(DumperAction::RETRACT);
+    doAction(DumperAction::LOWER);
+    in_startup_reset_ = false;
+    sendControlOff();
+}
+
+void DumperController::tryStartup()
+{
+    if (awakened_)
+        return;
+
+    std::cout << "[Init] Dumper first wake-up: resetting...\n";
+    onStartup();
+    awakened_ = true;
+}
+
+void DumperController::resetAwake()
+{
+    awakened_ = false;
+    last_dial_target_ = -1;
+    std::cout << "[INFO] Dumper awake state reset.\n";
+}
+
+void DumperController::updateDial(int new_dial, int current_state)
+{
+    if (in_startup_reset_)
+    {
+        std::cout << "[Dumper] In startup reset, ignoring remote control dump command.\n";
+        return;
+    }
+
+    if (new_dial < 0 || new_dial > 2)
+        return;
+
+    if (last_dial_target_ == new_dial)
+        return;
+
+    std::cout << "[Dial] Changed from " << current_state << " to " << new_dial << "\n";
+    handleDialChange(current_state, new_dial);
+    last_dial_target_ = new_dial;
+}
+
+// 拨钮变更处理（如从最下拨到最上，动作会排队）
+void DumperController::handleDialChange(int from, int to)
+{
+    std::vector<DumperAction> actions;
+
+    if (from == 0 && to == 1)
+        actions = {DumperAction::RAISE};
+    else if (from == 1 && to == 2)
+        actions = {DumperAction::DUMP};
+    else if (from == 2 && to == 1)
+        actions = {DumperAction::RETRACT};
+    else if (from == 1 && to == 0)
+        actions = {DumperAction::LOWER};
+    else if (from == 0 && to == 2)
+        actions = {DumperAction::RAISE, DumperAction::DUMP};
+    else if (from == 2 && to == 0)
+        actions = {DumperAction::RETRACT, DumperAction::LOWER};
+    else
+        return;
+
+    runActionSequence(actions);
+}
+
+// 执行动作序列，如果当前正在执行，则加入队列等待
+void DumperController::runActionSequence(const std::vector<DumperAction> &actions)
+{
+    if (busy_)
+    {
+        std::lock_guard<std::mutex> lock(queue_mutex_);
+        for (const auto &a : actions)
+            action_queue_.push(a);
+        has_pending_task_ = true;
+        return;
+    }
+
+    busy_ = true;
+    has_pending_task_ = false;
+
+    std::thread([this, actions]
+                {
+                    for (auto action : actions)
+                    {
+                        doAction(action);
+                    }
+
+                    // 执行排队的动作
+                    while (true)
+                    {
+                        DumperAction next;
+                        {
+                            std::lock_guard<std::mutex> lock(queue_mutex_);
+                            if (action_queue_.empty())
+                                break;
+                            next = action_queue_.front();
+                            action_queue_.pop();
+                        }
+                        doAction(next);
+                    }
+
+                    sendControlOff();
+                    busy_ = false; })
+        .detach();
+}
+
+// 每个动作的具体 CAN 指令 + 延时
+void DumperController::doAction(DumperAction act)
+{
+    constexpr int delay_ms = 10000; // 可统一调整延时时间（单位：毫秒）
+    using namespace std::chrono_literals;
+
+    CANFrame frame1;
+    CANFrame frame2;
+
+    switch (act)
+    {
+    case DumperAction::RAISE:
+        if (raised_)
+            return;
+        std::cout << "[Action] Raising...\n";
+        frame1 = {0x18FA1117, {0x00, 0x40, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
+        can_.sendFrame(frame1);
+        current_state_ = DumperState::RAISING;
+        std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+        raised_ = true;
+        dumped_ = false;
+        break;
+
+    case DumperAction::DUMP:
+        if (dumped_)
+            return;
+        std::cout << "[Action] Dumping...\n";
+        frame1 = {0x18FA1117, {0x00, 0x00, 0x00, 0x10, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
+        can_.sendFrame(frame1);
+        current_state_ = DumperState::DUMPING;
+        std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+        dumped_ = true;
+        break;
+
+    case DumperAction::RETRACT:
+        if (!dumped_)
+            return;
+        std::cout << "[Action] Retracting...\n";
+        frame1 = {0x18FA1117, {0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
+        frame2 = {0x18F21117, {0x00, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, 8, true, false};
+        can_.sendFrame(frame1);
+        can_.sendFrame(frame2);
+        current_state_ = DumperState::RETRACTING;
+        std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+        dumped_ = false;
+        break;
+
+    case DumperAction::LOWER:
+        if (!raised_)
+            return;
+        std::cout << "[Action] Lowering...\n";
+        frame1 = {0x18FA1117, {0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00}, 8, true, false};
+        frame2 = {0x18F21117, {0x00, 0x00, 0xA0, 0x00, 0x00, 0x00, 0x00, 0x00}, 8, true, false};
+        can_.sendFrame(frame1);
+        can_.sendFrame(frame2);
+        current_state_ = DumperState::LOWERING;
+        std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
+        raised_ = false;
+        break;
+    }
+    logState();
+}
+
+// 所有动作完成后关闭控制
+void DumperController::sendControlOff()
+{
+    CANFrame off1 = {0x18FA1117, {0, 0, 0, 0, 0, 0, 0, 0}, 8, true, false};
+    CANFrame off2 = {0x18F21117, {0, 0, 0, 0, 0, 0, 0, 0}, 8, true, false};
+    can_.sendFrame(off1);
+    can_.sendFrame(off2);
+    std::cout << "[INFO] Control OFF sent.\n";
+}
+
+void DumperController::logState() const
+{
+    const char *state_str[] = {"IDLE", "RAISING", "DUMPING", "RETRACTING", "LOWERING"};
+    std::cout << "[STATE] Dumper state: " << state_str[static_cast<int>(current_state_)] << "\n";
+}
+
+int DumperController::getSimpleState() const
+{
+    if (!raised_)
+        return 0; // 斗未升（已收）
+    else if (!dumped_)
+        return 1; // 斗升但未放斗
+    else
+        return 2; // 斗升且已放斗
+}

src/mc/src/mc.cpp

@@ -4,13 +4,18 @@
 #include "mc/can_driver.h"
 #include "mc/can_struct.h"
 #include "mc/get_config.h"
+#include "mc/dumper.h"
 #include "rclcpp/rclcpp.hpp"
 #include "sweeper_interfaces/msg/can_frame.hpp"
 #include "sweeper_interfaces/msg/mc_ctrl.hpp"
 
+// 全局开关，控制是否打印 CAN 消息到终端
+bool g_can_print_enable = false;
+
 namespace sweeperMsg = sweeper_interfaces::msg;
 
-CANDriver canctl;
+CANDriver canctl;                // can驱动实例
+DumperController dumper(canctl); // 斗控制器实例
 
 struct CanHandlerContext
 {
@@ -22,9 +27,6 @@ std::atomic<bool> vcu_msg_received = false; // 是否收到过vcu数据帧
 std::atomic<bool> vcu_awake = false;        // 是否唤醒成功
 rclcpp::Time last_vcu_msg_time;             // 上次收到vcu数据帧的时间
 
-// 全局开关，控制是否打印 CAN 消息到终端
-bool g_can_print_enable = true;
-
 struct ControlCache
 {
   std::mutex mutex; // 防止多线程冲突
@@ -183,8 +185,31 @@ void setupTimers(rclcpp::Node::SharedPtr node)
             RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] message timeout, resetting wake-up state.");
             vcu_msg_received.store(false);
             vcu_awake.store(false);
+            dumper.resetAwake();
+          }
+
+          if (vcu_awake.load())
+          {
+            dumper.tryStartup();
+
+            sweeperMsg::McCtrl msg = get_safe_control();
+
+            int to=static_cast<int>(msg.dump);
+            int from =dumper.getSimpleState();
+
+            dumper.updateDial(to, from);
           } });
 
+  //  DBS控制，100Hz
+  static rclcpp::TimerBase::SharedPtr timer_dbs = node->create_wall_timer(
+      std::chrono::milliseconds(10), []()
+      {
+        sweeperMsg::McCtrl msg = get_safe_control();
+
+        dbs_cmd.setEnabled(msg.ehb_anable);
+        dbs_cmd.setPressure(msg.ehb_brake_pressure);
+        canctl.sendFrame(dbs_cmd.toFrame()); });
+
   // MCU控制，50Hz
   static rclcpp::TimerBase::SharedPtr timer_mcu = node->create_wall_timer(
       std::chrono::milliseconds(20), []()

src/radio_ctrl/src/radio_ctrl.cpp

@@ -11,7 +11,6 @@ namespace sweeperMsg = sweeper_interfaces::msg;
 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};
 
 class SBUSNode : public rclcpp::Node
 {
@@ -63,11 +62,12 @@ private:
     {
         static int MCU_RPM_MAX = config.mcu_rpm_max;
         static float EPS_ANGLE_MAX = config.eps_angle_max;
+        static bool initialized = false; // 新增初始化标志
 
         bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性
 
         auto msg = sweeperMsg::McCtrl(); // 控制消息对象
-        uint16_t ch_data[8];             // 各通道遥控数据
+        uint16_t ch_data[10];            // 各通道遥控数据
 
         if (data_safe) // 数据安全，进行数据解析并发布
         {
@@ -79,42 +79,89 @@ private:
             }
             // printf("\n");
 
-            if (ch_data[6] == 192) // 是否使能车辆控制
+            uint16_t ctrl = ch_data[4];       // 手刹
+            uint16_t gear = ch_data[5];       // 挡位
+            uint16_t sweep = ch_data[6];      // 清扫
+            uint16_t dump = ch_data[7];       // 垃圾倾倒
+            int16_t speed = ch_data[1] - 992; //[-800,800]
+
+            if (!initialized)
+            {
+                if (sweep == 1792 && dump == 1792)
+                {
+                    initialized = true;
+                    RCLCPP_INFO(this->get_logger(), "[RADIO_CTRL] Initialize Success.");
+                }
+                else
+                {
+                    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 3000,
+                                         "[RADIO_CTRL] Please set sweep and dump to bottom to initialize.");
+                    return;
+                }
+            }
+
+            switch (dump)
+            {
+            case 1792:
+                msg.dump = 0;
+                break;
+            case 992:
+                msg.dump = 1;
+                break;
+            case 192:
+                msg.dump = 2;
+                break;
+            default:
+                break;
+            }
+
+            static uint16_t last_ctrl = 992; // 初始手刹值
+            static int current_brake = 1;    // 默认上电是拉手刹（1）
+
+            // 判断是否是上拨触发（192）
+            if (last_ctrl == 992 && ctrl == 192)
+            {
+                // 上拨 → 松手刹
+                current_brake = 0;
+            }
+            // 判断是否是下拨触发（1792）
+            else if (last_ctrl == 992 && ctrl == 1792)
+            {
+                // 下拨 → 拉手刹
+                current_brake = 1;
+            }
+
+            last_ctrl = ctrl;
+            msg.brake = current_brake;
+
+            if (msg.brake == 0) // 手刹释放
             {
-                msg.brake = 0;
                 // 挡位选择
-                if (ch_data[7] == 192)
+                if (gear == 192)
                 {
                     msg.gear = 0; // D挡
                 }
-                else if (ch_data[7] == 1792)
+                else if (gear == 1792)
                 {
                     msg.gear = 1; // R挡
                 }
-                else
-                {
-                    msg.gear = 0;
-                    msg.brake = 1;
-                    msg.rpm = 0;
-                }
 
-                if (ch_data[7] != 992)
-                {
                 // 油门 / 刹车逻辑
-                    if (ch_data[1] <= speed[1])
+                if (speed <= 0)
                 {
-                        msg.brake = 1;
+                    msg.ehb_anable = true;
+                    msg.ehb_brake_pressure = -0.01f * std::clamp(static_cast<int>(speed), -800, 0);
                     msg.rpm = 0;
                 }
                 else
                 {
-                        msg.brake = 0;
+                    msg.ehb_anable = false;
-                        msg.rpm = static_cast<uint8_t>(MCU_RPM_MAX * (ch_data[1] - speed[1]) / (speed[2] - speed[1]));
+                    msg.ehb_brake_pressure = 0;
-                    }
+                    msg.rpm = static_cast<uint8_t>(std::clamp(MCU_RPM_MAX * speed / 800, 0, MCU_RPM_MAX));
                 }
 
                 // 一键清扫
-                if (ch_data[5] == 1792)
+                if (sweep == 1792)
                 {
                     msg.edge_brush_lift = false;
                     msg.sweep_ctrl = false;
@@ -122,13 +169,13 @@ private:
                     msg.mud_flap = false;
                     msg.dust_shake = false;
                 }
-                else if (ch_data[5] == 192)
+                else if (sweep == 192)
                 {
                     msg.edge_brush_lift = true;
                     msg.sweep_ctrl = true;
                     msg.spray = true;
                     msg.mud_flap = true;
-                    msg.dust_shake = false;
+                    msg.dust_shake = true;
                 }
 
                 // 转向逻辑
@@ -149,7 +196,7 @@ private:
                 msg.angle = target_angle;
                 msg.angle_speed = can_speed;
             }
-            else // 未使能状态，发送安全默认控制指令
+            else // 未释放，发送安全默认控制指令
             {
                 msg.brake = 1;
                 msg.gear = 0;
@@ -171,7 +218,7 @@ private:
                                                        << "\n  gear: " << static_cast<int>(msg.gear)                               // uint8
                                                        << "\n  rpm: " << static_cast<int>(msg.rpm)                                 // uint8
                                                        << "\n  ehb_enable: " << static_cast<int>(msg.ehb_anable)                   // bool
-                                                       << "\n  ehb_brake_pressure: " << static_cast<int>(msg.ehb_brake_pressure) // uint8
+                                                       << "\n  ehb_brake_pressure: " << static_cast<float>(msg.ehb_brake_pressure) // float32
                                                        << "\n  angle: " << msg.angle                                               // float32
                                                        << "\n  angle_speed: " << msg.angle_speed                                   // uint16
                                                        << "\n  edge_brush_lift: " << static_cast<int>(msg.edge_brush_lift)         // bool
@@ -183,6 +230,7 @@ private:
                                                        << "\n  right_light: " << static_cast<int>(msg.right_light)                 // bool
                                                        << "\n  brake_light: " << static_cast<int>(msg.brake_light)                 // bool
                                                        << "\n  headlight: " << static_cast<int>(msg.headlight)                     // bool
+                                                       << "\n  dump: " << static_cast<int>(msg.dump)                               // uint8
             );
         }
         else

src/sweeper_interfaces/msg/McCtrl.msg

@@ -7,7 +7,7 @@ uint8 rpm	            #转速占空比 0-100
 
 #ehb部分
 bool ehb_anable                 #ehb使能
-uint8 ehb_brake_pressure        #ehb制动压力 0-8MPa
+float32 ehb_brake_pressure      #ehb制动压力 0-8MPa
 
 #eps部分
 float32 angle	    #轮端转向角度 分辨率0.2° [-66.0,66.0] 适当缩减
@@ -24,3 +24,6 @@ bool left_light     #左转向灯
 bool right_light    #右转向灯
 bool brake_light    #刹车灯
 bool headlight      #大灯
+
+#倒灰
+uint8 dump          # 0 全部收回 1 斗升 2 倒斗