From 1fd513fc1c71c97df495702d2b37dc24a37e09ea Mon Sep 17 00:00:00 2001 From: cxh Date: Wed, 18 Jun 2025 11:20:59 +0800 Subject: [PATCH] Auto commit at 2025-06-18 11:20:59 --- src/mc/include/mc/can_struct.h | 56 +++++++ src/mc/include/mc/dumper.h | 60 ++++++++ src/mc/src/can_struct.cpp | 1 + src/mc/src/dumper.cpp | 211 ++++++++++++++++++++++++++ src/mc/src/mc.cpp | 49 ++++-- src/radio_ctrl/src/radio_ctrl.cpp | 140 +++++++++++------ src/sweeper_interfaces/msg/McCtrl.msg | 7 +- 7 files changed, 464 insertions(+), 60 deletions(-) create mode 100644 src/mc/include/mc/dumper.h create mode 100644 src/mc/src/dumper.cpp diff --git a/src/mc/include/mc/can_struct.h b/src/mc/include/mc/can_struct.h index 9053f3e..3677f9d 100644 --- a/src/mc/include/mc/can_struct.h +++ b/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(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 diff --git a/src/mc/include/mc/dumper.h b/src/mc/include/mc/dumper.h new file mode 100644 index 0000000..55255a5 --- /dev/null +++ b/src/mc/include/mc/dumper.h @@ -0,0 +1,60 @@ +// 专门用于垃圾倾倒的头文件 +#ifndef DUMPER_H +#define DUMPER_H + +#include "can_driver.h" +#include +#include + +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 &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 action_queue_; + std::atomic busy_; + std::atomic has_pending_task_; + DumperState current_state_; + CANDriver &can_; +}; + +#endif \ No newline at end of file diff --git a/src/mc/src/can_struct.cpp b/src/mc/src/can_struct.cpp index 9e2659b..301bbe3 100644 --- a/src/mc/src/can_struct.cpp +++ b/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; diff --git a/src/mc/src/dumper.cpp b/src/mc/src/dumper.cpp new file mode 100644 index 0000000..d9e3ad0 --- /dev/null +++ b/src/mc/src/dumper.cpp @@ -0,0 +1,211 @@ +#include "mc/dumper.h" +#include +#include +#include + +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 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 &actions) +{ + if (busy_) + { + std::lock_guard 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 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(current_state_)] << "\n"; +} + +int DumperController::getSimpleState() const +{ + if (!raised_) + return 0; // 斗未升（已收） + else if (!dumped_) + return 1; // 斗升但未放斗 + else + return 2; // 斗升且已放斗 +} diff --git a/src/mc/src/mc.cpp b/src/mc/src/mc.cpp index 0b0988f..405a684 100644 --- a/src/mc/src/mc.cpp +++ b/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 vcu_msg_received = false; // 是否收到过vcu数据帧 std::atomic vcu_awake = false; // 是否唤醒成功 rclcpp::Time last_vcu_msg_time; // 上次收到vcu数据帧的时间 -// 全局开关，控制是否打印 CAN 消息到终端 -bool g_can_print_enable = true; - struct ControlCache { std::mutex mutex; // 防止多线程冲突 @@ -175,15 +177,38 @@ void setupTimers(rclcpp::Node::SharedPtr node) static rclcpp::TimerBase::SharedPtr timer_estop_watchdog = node->create_wall_timer( std::chrono::milliseconds(200), [node]() { - auto now = node->now(); - auto elapsed = now - last_vcu_msg_time; + auto now = node->now(); + auto elapsed = now - last_vcu_msg_time; - if (vcu_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5)) - { - RCLCPP_WARN(node->get_logger(), "[TIMER][VCU] message timeout, resetting wake-up state."); - vcu_msg_received.store(false); - vcu_awake.store(false); - } }); + if (vcu_msg_received.load() && elapsed > rclcpp::Duration::from_seconds(0.5)) + { + 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(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( diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index b87bfe3..0c8e43d 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/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) { - msg.brake = 0; - // 挡位选择 - if (ch_data[7] == 192) + if (sweep == 1792 && dump == 1792) { - msg.gear = 0; // D挡 - } - else if (ch_data[7] == 1792) - { - msg.gear = 1; // R挡 + initialized = true; + RCLCPP_INFO(this->get_logger(), "[RADIO_CTRL] Initialize Success."); } else { - msg.gear = 0; - msg.brake = 1; - msg.rpm = 0; + 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) // 手刹释放 + { + // 挡位选择 + if (gear == 192) + { + msg.gear = 0; // D挡 + } + else if (gear == 1792) + { + msg.gear = 1; // R挡 } - if (ch_data[7] != 992) + // 油门 / 刹车逻辑 + if (speed <= 0) { - // 油门 / 刹车逻辑 - 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])); - } + msg.ehb_anable = true; + msg.ehb_brake_pressure = -0.01f * std::clamp(static_cast(speed), -800, 0); + msg.rpm = 0; + } + else + { + msg.ehb_anable = false; + msg.ehb_brake_pressure = 0; + msg.rpm = static_cast(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; @@ -167,22 +214,23 @@ private: pub_->publish(msg); RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" - << "\n brake: " << static_cast(msg.brake) // uint8 - << "\n gear: " << static_cast(msg.gear) // uint8 - << "\n rpm: " << static_cast(msg.rpm) // uint8 - << "\n ehb_enable: " << static_cast(msg.ehb_anable) // bool - << "\n ehb_brake_pressure: " << static_cast(msg.ehb_brake_pressure) // uint8 - << "\n angle: " << msg.angle // float32 - << "\n angle_speed: " << msg.angle_speed // uint16 - << "\n edge_brush_lift: " << static_cast(msg.edge_brush_lift) // bool - << "\n sweep_ctrl: " << static_cast(msg.sweep_ctrl) // bool - << "\n spray: " << static_cast(msg.spray) // bool - << "\n mud_flap: " << static_cast(msg.mud_flap) // bool - << "\n dust_shake: " << static_cast(msg.dust_shake) // bool - << "\n left_light: " << static_cast(msg.left_light) // bool - << "\n right_light: " << static_cast(msg.right_light) // bool - << "\n brake_light: " << static_cast(msg.brake_light) // bool - << "\n headlight: " << static_cast(msg.headlight) // bool + << "\n brake: " << static_cast(msg.brake) // uint8 + << "\n gear: " << static_cast(msg.gear) // uint8 + << "\n rpm: " << static_cast(msg.rpm) // uint8 + << "\n ehb_enable: " << static_cast(msg.ehb_anable) // bool + << "\n ehb_brake_pressure: " << static_cast(msg.ehb_brake_pressure) // float32 + << "\n angle: " << msg.angle // float32 + << "\n angle_speed: " << msg.angle_speed // uint16 + << "\n edge_brush_lift: " << static_cast(msg.edge_brush_lift) // bool + << "\n sweep_ctrl: " << static_cast(msg.sweep_ctrl) // bool + << "\n spray: " << static_cast(msg.spray) // bool + << "\n mud_flap: " << static_cast(msg.mud_flap) // bool + << "\n dust_shake: " << static_cast(msg.dust_shake) // bool + << "\n left_light: " << static_cast(msg.left_light) // bool + << "\n right_light: " << static_cast(msg.right_light) // bool + << "\n brake_light: " << static_cast(msg.brake_light) // bool + << "\n headlight: " << static_cast(msg.headlight) // bool + << "\n dump: " << static_cast(msg.dump) // uint8 ); } else diff --git a/src/sweeper_interfaces/msg/McCtrl.msg b/src/sweeper_interfaces/msg/McCtrl.msg index 971a0f5..968965d 100644 --- a/src/sweeper_interfaces/msg/McCtrl.msg +++ b/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] 适当缩减 @@ -23,4 +23,7 @@ bool dust_shake #振尘 bool left_light #左转向灯 bool right_light #右转向灯 bool brake_light #刹车灯 -bool headlight #大灯 \ No newline at end of file +bool headlight #大灯 + +#倒灰 +uint8 dump # 0 全部收回 1 斗升 2 倒斗 \ No newline at end of file