diff --git a/src/ctrl_arbiter/src/ctrl_arbiter.cpp b/src/ctrl_arbiter/src/ctrl_arbiter.cpp index 2f6f6ae..69038d4 100644 --- a/src/ctrl_arbiter/src/ctrl_arbiter.cpp +++ b/src/ctrl_arbiter/src/ctrl_arbiter.cpp @@ -85,13 +85,7 @@ private: safe_msg.angle = 0; safe_msg.angle_speed = 120; - safe_msg.roll_brush_suction_direction = 0; - safe_msg.roll_brush_suction = 0; - safe_msg.side_brush = 0; - safe_msg.dust_shake = 0; - safe_msg.brush_deck_pusher = 0; - safe_msg.suction_squeeqee_pusher = 0; - safe_msg.water_spray = 0; + safe_msg.sweep = false; publisher_->publish(safe_msg); RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, diff --git a/src/mc/include/mc/can_struct.h b/src/mc/include/mc/can_struct.h index 537025b..059c05c 100644 --- a/src/mc/include/mc/can_struct.h +++ b/src/mc/include/mc/can_struct.h @@ -184,7 +184,7 @@ struct can_VCU_enable_cmd static constexpr bool EXT_FLAG = false; static constexpr bool RTR_FLAG = false; - uint8_t data[8]{}; + int8_t data[8]{}; // 设置CAN指令使能 void setCANEnable(bool enable) @@ -213,54 +213,54 @@ struct can_VCU_motor1_cmd static constexpr bool EXT_FLAG = false; static constexpr bool RTR_FLAG = false; - uint8_t data[8]{}; + int8_t data[8]{}; - // 设置电机M1方向 (滚刷&吸风电机) - Byte 0 - void setM1Direction(uint8_t direction) + // 设置Byte0 + void setByte0(int8_t direction) { - data[0] = std::clamp(direction, static_cast(0), static_cast(2)); + data[0] = std::clamp(direction, static_cast(0), static_cast(2)); } - // 设置电机M1油门 (滚刷&吸风电机) - Byte 1 - void setM1Throttle(uint8_t throttle) + // 设置Byte1 + void setByte1(int8_t value) { - data[1] = std::clamp(throttle, static_cast(0), static_cast(100)); + data[1] = std::clamp(value, static_cast(0), static_cast(100)); } - // 设置电机M2 (边刷电机) - Byte 2 - void setM2Speed(int8_t speed) + // 设置Byte2 + void setByte2(int8_t value) { - data[2] = static_cast(std::clamp(speed, static_cast(0), static_cast(100))); + data[2] = std::clamp(value, static_cast(0), static_cast(100)); } - // 设置电机M3 (振尘电机) - Byte 3 - void setM3Speed(int8_t speed) + // 设置Byte3 + void setByte3(int8_t value) { - data[3] = static_cast(std::clamp(speed, static_cast(0), static_cast(100))); + data[3] = std::clamp(value, static_cast(0), static_cast(100)); } - // 设置电机M4 (刷盘推杆电机) - Byte 4 - void setM4Speed(int8_t speed) + // 设置Byte4 + void setByte4(int8_t value) { - data[4] = static_cast(std::clamp(speed, static_cast(-100), static_cast(100))); + data[4] = std::clamp(value, static_cast(0), static_cast(100)); } - // 设置电机M5 (吸扒推杆电机) - Byte 5 - void setM5Speed(int8_t speed) + // 设置Byte5 + void setByte5(int8_t value) { - data[5] = static_cast(std::clamp(speed, static_cast(-100), static_cast(100))); + data[5] = std::clamp(value, static_cast(0), static_cast(100)); } - // 设置电机M6 (LED_OUT6) - Byte 6 - void setM6_LED6(int8_t value) + // 设置Byte6 + void setByte6(int8_t value) { - data[6] = static_cast(std::clamp(value, static_cast(0), static_cast(100))); + data[6] = std::clamp(value, static_cast(0), static_cast(100)); } - // 设置电机M7 (LED_OUT7) - Byte 7 - void setM7_LED7(int8_t value) + // 设置Byte7 + void setByte7(int8_t value) { - data[7] = static_cast(std::clamp(value, static_cast(0), static_cast(100))); + data[7] = std::clamp(value, static_cast(0), static_cast(100)); } CANFrame toFrame() const @@ -282,54 +282,54 @@ struct can_VCU_motor2_cmd static constexpr bool EXT_FLAG = false; static constexpr bool RTR_FLAG = false; - uint8_t data[8]{}; + int8_t data[8]{}; - // 设置电机M8 (喷水电机) - Byte 0 - void setM8_Spray(int8_t speed) + // 设置Byte0 + void setByte0(int8_t value) { - data[0] = static_cast(std::clamp(speed, static_cast(-100), static_cast(100))); + data[0] = std::clamp(value, static_cast(0), static_cast(100)); } - // 设置OUT1 (预留20mA) - Byte 1 - void setOUT1_Reserved(uint8_t value) + // 设置Byte1 + void setByte1(int8_t value) { - data[1] = value; // 预留字段 + data[1] = std::clamp(value, static_cast(0), static_cast(100)); } - // 设置OUT2 (LED_OUT1) - Byte 2 - void setOUT2_LED1(uint8_t brightness) + // 设置Byte2 + void setByte2(int8_t value) { - data[2] = std::clamp(brightness, static_cast(0), static_cast(100)); + data[2] = std::clamp(value, static_cast(0), static_cast(100)); } - // 设置OUT3 (LED_OUT2) - Byte 3 - void setOUT3_LED2(uint8_t brightness) + // 设置Byte3 + void setByte3(int8_t value) { - data[3] = std::clamp(brightness, static_cast(0), static_cast(100)); + data[3] = std::clamp(value, static_cast(0), static_cast(100)); } - // 设置OUT4 (LED_OUT3) - Byte 4 - void setOUT4_LED3(uint8_t brightness) + // 设置Byte4 + void setByte4(int8_t value) { - data[4] = std::clamp(brightness, static_cast(0), static_cast(100)); + data[4] = std::clamp(value, static_cast(0), static_cast(100)); } - // 设置OUT5 (LED_OUT4) - Byte 5 - void setOUT5_LED4(uint8_t brightness) + // 设置Byte5 + void setByte5(int8_t value) { - data[5] = std::clamp(brightness, static_cast(0), static_cast(100)); + data[5] = std::clamp(value, static_cast(0), static_cast(100)); } - // 设置OUT6 (LED_OUT5) - Byte 6 - void setOUT6_LED5(uint8_t brightness) + // 设置Byte6 + void setByte6(int8_t value) { - data[6] = std::clamp(brightness, static_cast(0), static_cast(100)); + data[6] = std::clamp(value, static_cast(0), static_cast(100)); } - // Byte 7 未使用，保持为0 - void clearByte7() + // 设置Byte7 + void setByte7(int8_t value) { - data[7] = 100; + data[7] = std::clamp(value, static_cast(0), static_cast(100)); } CANFrame toFrame() const diff --git a/src/mc/src/control_cache.cpp b/src/mc/src/control_cache.cpp index 35b8e02..d138d37 100644 --- a/src/mc/src/control_cache.cpp +++ b/src/mc/src/control_cache.cpp @@ -35,13 +35,7 @@ sweeper_interfaces::msg::McCtrl get_safe_control() msg.angle = 0; msg.angle_speed = 120; - msg.roll_brush_suction_direction = 0; - msg.roll_brush_suction = 0; - msg.side_brush = 0; - msg.dust_shake = 0; - msg.brush_deck_pusher = 0; - msg.suction_squeeqee_pusher = 0; - msg.water_spray = 0; + msg.sweep = false; } return msg; } diff --git a/src/mc/src/mc.cpp b/src/mc/src/mc.cpp index 0832f7f..a62bbc2 100644 --- a/src/mc/src/mc.cpp +++ b/src/mc/src/mc.cpp @@ -15,19 +15,26 @@ CANDriver canctl; void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg) { - std::cout << "Sub ControlMsg:" - << "\n 电磁刹: " << static_cast(msg->brake) - << "\n 挡位: " << static_cast(msg->gear) - << "\n 转速: " << static_cast(msg->rpm) - << "\n 轮端转向角度: " << msg->angle - << "\n 转向角速度: " << msg->angle_speed - << "\n 滚刷&吸风电机方向: " << static_cast(msg->roll_brush_suction_direction) - << "\n 滚刷&吸风电机: " << static_cast(msg->roll_brush_suction) - << "\n 边刷电机: " << static_cast(msg->side_brush) - << "\n 振尘电机: " << static_cast(msg->dust_shake) - << "\n 刷盘推杆电机: " << static_cast(msg->brush_deck_pusher) - << "\n 吸扒推杆电机: " << static_cast(msg->suction_squeeqee_pusher) - << "\n 喷水电机: " << static_cast(msg->water_spray) + std::cout << "\n 刹车: " << (msg->brake ? "已刹车" : "未刹车") + << "\n 挡位: "; + switch (msg->gear) + { + case 0: + std::cout << "空挡"; + break; + case 1: + std::cout << "前进挡"; + break; + case 2: + std::cout << "后退挡"; + break; + default: + std::cout << "未知挡位(" << static_cast(msg->gear) << ")"; + break; + } + std::cout << "\n 行走电机转速: " << static_cast(msg->rpm) << " RPM" + << "\n 轮端转向角度: " << msg->angle << "°" + << "\n 清扫状态: " << (msg->sweep ? "正在清扫" : "未清扫") << std::endl; control_cache.update(*msg); diff --git a/src/mc/src/timer_tasks.cpp b/src/mc/src/timer_tasks.cpp index 1b94f0c..38fcfc2 100644 --- a/src/mc/src/timer_tasks.cpp +++ b/src/mc/src/timer_tasks.cpp @@ -25,17 +25,11 @@ void epsTimerTask(CANDriver &canctl) } // 定时器回调：VCU 扫地控制 +// 修改timer_tasks.cpp中的vcuTimerTask函数 void vcuTimerTask(CANDriver &canctl) { static bool vcu_initialized = false; - - static int last_roll_brush_suction_direction = 0; - static int last_roll_brush_suction = 0; - static int last_side_brush = 0; - static int last_dust_shake = 0; - static int last_brush_deck_pusher = 0; - static int last_suction_squeeqee_pusher = 0; - static int last_water_spray = 0; + static bool last_sweep_state = false; // 记录上一次清扫状态 auto msg = get_safe_control(); @@ -49,56 +43,42 @@ void vcuTimerTask(CANDriver &canctl) RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[VCU] CAN control enabled"); } - // 检查状态变化 - bool roll_brush_suction_direction_changed = (msg.roll_brush_suction_direction != last_roll_brush_suction_direction); - bool roll_brush_suction_changed = (msg.roll_brush_suction != last_roll_brush_suction); - bool side_brush_changed = (msg.side_brush != last_side_brush); - bool dust_shake_changed = (msg.dust_shake != last_dust_shake); - bool brush_deck_pusher_changed = (msg.brush_deck_pusher != last_brush_deck_pusher); - bool suction_squeeqee_pusher_changed = (msg.suction_squeeqee_pusher != last_suction_squeeqee_pusher); - bool water_spray_changed = (msg.water_spray != last_water_spray); + // 检查清扫状态是否变化 + bool sweep_state_changed = (msg.sweep != last_sweep_state); - bool motor1_needs_update = roll_brush_suction_direction_changed || roll_brush_suction_changed || side_brush_changed || dust_shake_changed || brush_deck_pusher_changed || suction_squeeqee_pusher_changed || water_spray_changed; - bool motor2_needs_update = water_spray_changed; - - // ===== 控制0x211报文 (电机M1-M7) ===== - if (motor1_needs_update) + if (sweep_state_changed) { - vcu_motor1_cmd.setM1Direction(msg.roll_brush_suction_direction); - vcu_motor1_cmd.setM1Throttle(msg.roll_brush_suction); - vcu_motor1_cmd.setM2Speed(msg.side_brush); - vcu_motor1_cmd.setM3Speed(msg.dust_shake); - vcu_motor1_cmd.setM4Speed(msg.brush_deck_pusher); - vcu_motor1_cmd.setM5Speed(msg.suction_squeeqee_pusher); - vcu_motor1_cmd.setM6_LED6(100); - vcu_motor1_cmd.setM7_LED7(100); + // 根据清扫状态设置所有部件 + uint8_t target_value = msg.sweep ? 100 : 0; // 100表示启动，0表示停止 + // ===== 控制0x211报文 (电机M1-M7) ===== + vcu_motor1_cmd.setByte0(msg.sweep ? 1 : 0); // 1表示正向运行，0表示停止 + vcu_motor1_cmd.setByte1(target_value); + vcu_motor1_cmd.setByte2(target_value); + vcu_motor1_cmd.setByte3(target_value); + vcu_motor1_cmd.setByte4(target_value); + vcu_motor1_cmd.setByte5(target_value); + vcu_motor1_cmd.setByte6(target_value); + vcu_motor1_cmd.setByte7(target_value); canctl.sendFrame(vcu_motor1_cmd.toFrame()); - } - - // ===== 控制0x212报文 (电机M8和LED输出) ===== - if (motor2_needs_update) - { - // 喷水电机控制 - vcu_motor2_cmd.setM8_Spray(msg.water_spray); - vcu_motor2_cmd.setOUT1_Reserved(100); - vcu_motor2_cmd.setOUT2_LED1(100); - vcu_motor2_cmd.setOUT3_LED2(100); - vcu_motor2_cmd.setOUT4_LED3(100); - vcu_motor2_cmd.setOUT5_LED4(100); - vcu_motor2_cmd.setOUT6_LED5(100); + // ===== 控制0x212报文 (电机M8和LED输出) ===== + vcu_motor2_cmd.setByte0(target_value); + vcu_motor2_cmd.setByte1(target_value); + vcu_motor2_cmd.setByte2(target_value); + vcu_motor2_cmd.setByte3(target_value); + vcu_motor2_cmd.setByte4(target_value); + vcu_motor2_cmd.setByte5(target_value); + vcu_motor2_cmd.setByte6(target_value); + vcu_motor2_cmd.setByte7(target_value); canctl.sendFrame(vcu_motor2_cmd.toFrame()); - } - // 更新状态记录 - last_roll_brush_suction_direction = msg.roll_brush_suction_direction; - last_roll_brush_suction = msg.roll_brush_suction; - last_side_brush = msg.side_brush; - last_dust_shake = msg.dust_shake; - last_brush_deck_pusher = msg.brush_deck_pusher; - last_suction_squeeqee_pusher = msg.suction_squeeqee_pusher; - last_water_spray = msg.water_spray; + RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), + "[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated"); + + // 更新状态记录 + last_sweep_state = msg.sweep; + } } // 注册所有定时器任务 diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index c6de8f1..a977054 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/src/radio_ctrl/src/radio_ctrl.cpp @@ -112,24 +112,12 @@ private: // 不清扫 if (sweep == 1792) { - msg.roll_brush_suction_direction = 0; - msg.roll_brush_suction = 0; - msg.side_brush = 0; - msg.dust_shake = 0; - msg.brush_deck_pusher = 0; - msg.suction_squeeqee_pusher = 0; - msg.water_spray = 0; + msg.sweep = false; } // 清扫 else if (sweep == 192) { - msg.roll_brush_suction_direction = 1; - msg.roll_brush_suction = 100; - msg.side_brush = 100; - msg.dust_shake = 100; - msg.brush_deck_pusher = 100; - msg.suction_squeeqee_pusher = 100; - msg.water_spray = 100; + msg.sweep = true; } // 转向逻辑 @@ -172,7 +160,7 @@ private: } std::cout << "\n 行走电机转速: " << static_cast(msg.rpm) << " RPM" << "\n 轮端转向角度: " << msg.angle << "°" - << "\n 清扫状态: " << (msg.roll_brush_suction_direction ? "正在清扫" : "未清扫") + << "\n 清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫") << std::endl; } else diff --git a/src/sweeper_interfaces/msg/McCtrl.msg b/src/sweeper_interfaces/msg/McCtrl.msg index 624aa1f..c539cf8 100644 --- a/src/sweeper_interfaces/msg/McCtrl.msg +++ b/src/sweeper_interfaces/msg/McCtrl.msg @@ -10,10 +10,4 @@ float32 angle #轮端转向角度 分辨率0.2° [-66.0,66.0] 适当缩减 uint16 angle_speed #转向角速度 120-1500rpm #vcu部分 -uint8 roll_brush_suction_direction #滚刷&吸风电机方向 0：不转动 1：正转 2：反转 -uint8 roll_brush_suction #滚刷&吸风电机 0 ~ 100 -int8 side_brush #边刷电机 0 ~ 100 -int8 dust_shake #振尘电机 0 ~ 100 -int8 brush_deck_pusher #刷盘推杆电机 -100 ~ 100 -int8 suction_squeeqee_pusher #吸扒推杆电机 -100 ~ 100 -int8 water_spray #喷水电机 -100 ~ 100 +bool sweep #清扫 true:清扫 false:不清扫