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_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,

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 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<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
void setM1Throttle(uint8_t throttle)
// 设置Byte1
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
void setM2Speed(int8_t speed)
// 设置Byte2
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
void setM3Speed(int8_t speed)
// 设置Byte3
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
void setM4Speed(int8_t speed)
// 设置Byte4
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
void setM5Speed(int8_t speed)
// 设置Byte5
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
void setM6_LED6(int8_t value)
// 设置Byte6
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
void setM7_LED7(int8_t value)
// 设置Byte7
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
@ -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<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
void setOUT1_Reserved(uint8_t value)
// 设置Byte1
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
void setOUT2_LED1(uint8_t brightness)
// 设置Byte2
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
void setOUT3_LED2(uint8_t brightness)
// 设置Byte3
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
void setOUT4_LED3(uint8_t brightness)
// 设置Byte4
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
void setOUT5_LED4(uint8_t brightness)
// 设置Byte5
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
void setOUT6_LED5(uint8_t brightness)
// 设置Byte6
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
void clearByte7()
// 设置Byte7
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

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_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;
}

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

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

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

@ -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;
if (sweep_state_changed)
{
// 根据清扫状态设置所有部件
uint8_t target_value = msg.sweep ? 100 : 0; // 100表示启动，0表示停止
// ===== 控制0x211报文 (电机M1-M7) =====
if (motor1_needs_update)
{
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);
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);
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());
}
RCLCPP_INFO(rclcpp::get_logger("timer_tasks"),
"[VCU] Sweep mode %s", msg.sweep ? "activated" : "deactivated");
// 更新状态记录
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;
last_sweep_state = msg.sweep;
}
}
// 注册所有定时器任务

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

@ -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<int>(msg.rpm) << " RPM"
<< "\n 轮端转向角度: " << msg.angle << "°"
<< "\n 清扫状态: " << (msg.roll_brush_suction_direction ? "正在清扫" : "未清扫")
<< "\n 清扫状态: " << (msg.sweep ? "正在清扫" : "未清扫")
<< std::endl;
}
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
#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:不清扫