lyq/sweeper_whu
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix : DaNeng frame 2

Browse Source
This commit is contained in:
Alvin-lyq 2025-09-15 10:08:34 +08:00
parent 80f7e45f56
commit f95db21402
7 changed files with 108 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/ctrl_arbiter/src/ctrl_arbiter.cpp
View File

@ -85,13 +85,7 @@ private:
safe_msg.angle = 0; safe_msg.angle = 0;
safe_msg.angle_speed = 120; safe_msg.angle_speed = 120;
safe_msg.roll_brush_suction_direction = 0; safe_msg.sweep = false;
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;
publisher_->publish(safe_msg); publisher_->publish(safe_msg);
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,

102
src/mc/include/mc/can_struct.h
View File

@ -184,7 +184,7 @@ struct can_VCU_enable_cmd
static constexpr bool EXT_FLAG = false; static constexpr bool EXT_FLAG = false;
static constexpr bool RTR_FLAG = false; static constexpr bool RTR_FLAG = false;
uint8_t data[8]{}; int8_t data[8]{};
// 设置CAN指令使能 // 设置CAN指令使能
void setCANEnable(bool enable) void setCANEnable(bool enable)
@ -213,54 +213,54 @@ struct can_VCU_motor1_cmd
static constexpr bool EXT_FLAG = false; static constexpr bool EXT_FLAG = false;
static constexpr bool RTR_FLAG = false; static constexpr bool RTR_FLAG = false;
uint8_t data[8]{}; int8_t data[8]{};
// 设置电机M1方向 (滚刷&吸风电机) - Byte 0 // 设置Byte0
void setM1Direction(uint8_t direction) void setByte0(int8_t direction)
{ {
data[0] = std::clamp(direction, static_cast<uint8_t>(0), static_cast<uint8_t>(2)); data[0] = std::clamp(direction, static_cast<int8_t>(0), static_cast<int8_t>(2));
} }
// 设置电机M1油门 (滚刷&吸风电机) - Byte 1 // 设置Byte1
void setM1Throttle(uint8_t throttle) void setByte1(int8_t value)
{ {
data[1] = std::clamp(throttle, static_cast<uint8_t>(0), static_cast<uint8_t>(100)); data[1] = std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100));
} }
// 设置电机M2 (边刷电机) - Byte 2 // 设置Byte2
void setM2Speed(int8_t speed) void setByte2(int8_t value)
{ {
data[2] = static_cast<uint8_t>(std::clamp(speed, static_cast<int8_t>(0), static_cast<int8_t>(100))); data[2] = std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100));
} }
// 设置电机M3 (振尘电机) - Byte 3 // 设置Byte3
void setM3Speed(int8_t speed) void setByte3(int8_t value)
{ {
data[3] = static_cast<uint8_t>(std::clamp(speed, static_cast<int8_t>(0), static_cast<int8_t>(100))); data[3] = std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100));
} }
// 设置电机M4 (刷盘推杆电机) - Byte 4 // 设置Byte4
void setM4Speed(int8_t speed) void setByte4(int8_t value)
{ {
data[4] = static_cast<uint8_t>(std::clamp(speed, static_cast<int8_t>(-100), static_cast<int8_t>(100))); data[4] = std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100));
} }
// 设置电机M5 (吸扒推杆电机) - Byte 5 // 设置Byte5
void setM5Speed(int8_t speed) void setByte5(int8_t value)
{ {
data[5] = static_cast<uint8_t>(std::clamp(speed, static_cast<int8_t>(-100), static_cast<int8_t>(100))); data[5] = std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100));
} }
// 设置电机M6 (LED_OUT6) - Byte 6 // 设置Byte6
void setM6_LED6(int8_t value) void setByte6(int8_t value)
{ {
data[6] = static_cast<uint8_t>(std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100))); data[6] = std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100));
} }
// 设置电机M7 (LED_OUT7) - Byte 7 // 设置Byte7
void setM7_LED7(int8_t value) void setByte7(int8_t value)
{ {
data[7] = static_cast<uint8_t>(std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100))); data[7] = std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100));
} }
CANFrame toFrame() const CANFrame toFrame() const
@ -282,54 +282,54 @@ struct can_VCU_motor2_cmd
static constexpr bool EXT_FLAG = false; static constexpr bool EXT_FLAG = false;
static constexpr bool RTR_FLAG = false; static constexpr bool RTR_FLAG = false;
uint8_t data[8]{}; int8_t data[8]{};
// 设置电机M8 (喷水电机) - Byte 0 // 设置Byte0
void setM8_Spray(int8_t speed) void setByte0(int8_t value)
{ {
data[0] = static_cast<uint8_t>(std::clamp(speed, static_cast<int8_t>(-100), static_cast<int8_t>(100))); data[0] = std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100));
} }
// 设置OUT1 (预留20mA) - Byte 1 // 设置Byte1
void setOUT1_Reserved(uint8_t value) void setByte1(int8_t value)
{ {
data[1] = value; // 预留字段 data[1] = std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100));
} }
// 设置OUT2 (LED_OUT1) - Byte 2 // 设置Byte2
void setOUT2_LED1(uint8_t brightness) void setByte2(int8_t value)
{ {
data[2] = std::clamp(brightness, static_cast<uint8_t>(0), static_cast<uint8_t>(100)); data[2] = std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100));
} }
// 设置OUT3 (LED_OUT2) - Byte 3 // 设置Byte3
void setOUT3_LED2(uint8_t brightness) void setByte3(int8_t value)
{ {
data[3] = std::clamp(brightness, static_cast<uint8_t>(0), static_cast<uint8_t>(100)); data[3] = std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100));
} }
// 设置OUT4 (LED_OUT3) - Byte 4 // 设置Byte4
void setOUT4_LED3(uint8_t brightness) void setByte4(int8_t value)
{ {
data[4] = std::clamp(brightness, static_cast<uint8_t>(0), static_cast<uint8_t>(100)); data[4] = std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100));
} }
// 设置OUT5 (LED_OUT4) - Byte 5 // 设置Byte5
void setOUT5_LED4(uint8_t brightness) void setByte5(int8_t value)
{ {
data[5] = std::clamp(brightness, static_cast<uint8_t>(0), static_cast<uint8_t>(100)); data[5] = std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100));
} }
// 设置OUT6 (LED_OUT5) - Byte 6 // 设置Byte6
void setOUT6_LED5(uint8_t brightness) void setByte6(int8_t value)
{ {
data[6] = std::clamp(brightness, static_cast<uint8_t>(0), static_cast<uint8_t>(100)); data[6] = std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100));
} }
// Byte 7 未使用，保持为0 // 设置Byte7
void clearByte7() void setByte7(int8_t value)
{ {
data[7] = 100; data[7] = std::clamp(value, static_cast<int8_t>(0), static_cast<int8_t>(100));
} }
CANFrame toFrame() const CANFrame toFrame() const

8
src/mc/src/control_cache.cpp
View File

@ -35,13 +35,7 @@ sweeper_interfaces::msg::McCtrl get_safe_control()
msg.angle = 0; msg.angle = 0;
msg.angle_speed = 120; msg.angle_speed = 120;
msg.roll_brush_suction_direction = 0; msg.sweep = false;
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;
} }
return msg; return msg;
} }

33
src/mc/src/mc.cpp
View File

@ -15,19 +15,26 @@ CANDriver canctl;
void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg) void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg)
{ {
std::cout << "Sub ControlMsg:" std::cout << "\n 刹车: " << (msg->brake ? "已刹车" : "未刹车")
<< "\n 电磁刹: " << static_cast<int>(msg->brake) << "\n 挡位: ";
<< "\n 挡位: " << static_cast<int>(msg->gear) switch (msg->gear)
<< "\n 转速: " << static_cast<int>(msg->rpm) {
<< "\n 轮端转向角度: " << msg->angle case 0:
<< "\n 转向角速度: " << msg->angle_speed std::cout << "空挡";
<< "\n 滚刷&吸风电机方向: " << static_cast<int>(msg->roll_brush_suction_direction) break;
<< "\n 滚刷&吸风电机: " << static_cast<int>(msg->roll_brush_suction) case 1:
<< "\n 边刷电机: " << static_cast<int>(msg->side_brush) std::cout << "前进挡";
<< "\n 振尘电机: " << static_cast<int>(msg->dust_shake) break;
<< "\n 刷盘推杆电机: " << static_cast<int>(msg->brush_deck_pusher) case 2:
<< "\n 吸扒推杆电机: " << static_cast<int>(msg->suction_squeeqee_pusher) std::cout << "后退挡";
<< "\n 喷水电机: " << static_cast<int>(msg->water_spray) break;
default:
std::cout << "未知挡位(" << static_cast<int>(msg->gear) << ")";
break;
}
std::cout << "\n 行走电机转速: " << static_cast<int>(msg->rpm) << " RPM"
<< "\n 轮端转向角度: " << msg->angle << "°"
<< "\n 清扫状态: " << (msg->sweep ? "正在清扫" : "未清扫")
<< std::endl; << std::endl;
control_cache.update(*msg); control_cache.update(*msg);

82
src/mc/src/timer_tasks.cpp
View File

@ -25,17 +25,11 @@ void epsTimerTask(CANDriver &canctl)
} }
// 定时器回调：VCU 扫地控制 // 定时器回调：VCU 扫地控制
// 修改timer_tasks.cpp中的vcuTimerTask函数
void vcuTimerTask(CANDriver &canctl) void vcuTimerTask(CANDriver &canctl)
{ {
static bool vcu_initialized = false; static bool vcu_initialized = false;
static bool last_sweep_state = 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;
auto msg = get_safe_control(); auto msg = get_safe_control();
@ -49,56 +43,42 @@ void vcuTimerTask(CANDriver &canctl)
RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[VCU] CAN control enabled"); 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 sweep_state_changed = (msg.sweep != last_sweep_state);
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 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; if (sweep_state_changed)
bool motor2_needs_update = water_spray_changed;
// ===== 控制0x211报文 (电机M1-M7) =====
if (motor1_needs_update)
{ {
vcu_motor1_cmd.setM1Direction(msg.roll_brush_suction_direction); // 根据清扫状态设置所有部件
vcu_motor1_cmd.setM1Throttle(msg.roll_brush_suction); uint8_t target_value = msg.sweep ? 100 : 0; // 100表示启动，0表示停止
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);
// ===== 控制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()); 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()); canctl.sendFrame(vcu_motor2_cmd.toFrame());
}
// 更新状态记录 RCLCPP_INFO(rclcpp::get_logger("timer_tasks"),
last_roll_brush_suction_direction = msg.roll_brush_suction_direction; "[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated");
last_roll_brush_suction = msg.roll_brush_suction;
last_side_brush = msg.side_brush; // 更新状态记录
last_dust_shake = msg.dust_shake; last_sweep_state = msg.sweep;
last_brush_deck_pusher = msg.brush_deck_pusher; }
last_suction_squeeqee_pusher = msg.suction_squeeqee_pusher;
last_water_spray = msg.water_spray;
} }
// 注册所有定时器任务 // 注册所有定时器任务

18
src/radio_ctrl/src/radio_ctrl.cpp
View File

@ -112,24 +112,12 @@ private:
// 不清扫 // 不清扫
if (sweep == 1792) if (sweep == 1792)
{ {
msg.roll_brush_suction_direction = 0; msg.sweep = false;
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;
} }
// 清扫 // 清扫
else if (sweep == 192) else if (sweep == 192)
{ {
msg.roll_brush_suction_direction = 1; msg.sweep = true;
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;
} }
// 转向逻辑 // 转向逻辑
@ -172,7 +160,7 @@ private:
} }
std::cout << "\n 行走电机转速: " << static_cast<int>(msg.rpm) << " RPM" std::cout << "\n 行走电机转速: " << static_cast<int>(msg.rpm) << " RPM"
<< "\n 轮端转向角度: " << msg.angle << "°" << "\n 轮端转向角度: " << msg.angle << "°"
<< "\n 清扫状态: " << (msg.roll_brush_suction_direction ? "正在清扫" : "未清扫") << "\n 清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫")
<< std::endl; << std::endl;
} }
else else

8
src/sweeper_interfaces/msg/McCtrl.msg
View File

@ -10,10 +10,4 @@ float32 angle #轮端转向角度 分辨率0.2° [-66.0,66.0] 适当缩减
uint16 angle_speed #转向角速度 120-1500rpm uint16 angle_speed #转向角速度 120-1500rpm
#vcu部分 #vcu部分
uint8 roll_brush_suction_direction #滚刷&吸风电机方向 0：不转动 1：正转 2：反转 bool sweep #清扫 true:清扫 false:不清扫
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