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

Compare commits

base: lyq:a5aef934328248711664375160b94f2aedb96887
Branches Tags
lyq:master
lyq:humble
lyq:main
..
compare: lyq:45327d34ca078330d4214aa79200d324cdded4cd
Branches Tags
lyq:humble
lyq:main
lyq:master

No commits in common. "a5aef934328248711664375160b94f2aedb96887" and "45327d34ca078330d4214aa79200d324cdded4cd" have entirely different histories.
a5aef93432 ... 45327d34ca

1 changed files with 39 additions and 28 deletions
Show Stats Expand all files Collapse all files

67
src/control/can_radio_ctrl/src/can_radio_ctrl.cpp
View File

@ -74,35 +74,36 @@ class CanRadioCtrlNode : public rclcpp::Node
if (has_received && enabled) if (has_received && enabled)
{ {
if (radio_data.throttle > 0) // 解析挡位
switch (radio_data.gear) // 通道E
{ {
// 油门 > 0 → 前进挡 D case 0:
msg.gear = 2; msg.gear = 1;
msg.brake = 0; break; // R挡
case 1:
// 计算前进转速 msg.gear = 0;
float throttle_ratio = static_cast<float>(radio_data.throttle) / 450.0f; break; // N挡
int rpm_val = static_cast<int>(mcu_rpm_max_ * throttle_ratio); case 2:
msg.rpm = static_cast<uint16_t>(std::clamp(rpm_val, 0, mcu_rpm_max_)); msg.gear = 2;
break; // D挡
default:
msg.gear = 0;
LOG_WARN("Unknown gear: %d", radio_data.gear);
break;
} }
else if (radio_data.throttle < 0)
{
// 油门 < 0 → 后退挡 R
msg.gear = 1;
msg.brake = 0;
// 取绝对值计算后退转速（后退速度和前进一致） // 解析油门/刹车
float throttle_abs = fabs(radio_data.throttle); if (radio_data.throttle <= 0)
float throttle_ratio = static_cast<float>(throttle_abs) / 450.0f; {
int rpm_val = static_cast<int>(mcu_rpm_max_ * throttle_ratio); msg.brake = 1;
msg.rpm = static_cast<uint16_t>(std::clamp(rpm_val, 0, mcu_rpm_max_)); msg.rpm = 0;
} }
else else
{ {
// 油门 = 0 → 空挡 + 刹车 msg.brake = 0;
msg.gear = 0; float throttle_ratio = static_cast<float>(radio_data.throttle) / 450.0f;
msg.brake = 1; int rpm_val = static_cast<int>(mcu_rpm_max_ * throttle_ratio);
msg.rpm = 0; msg.rpm = static_cast<uint16_t>(std::clamp(rpm_val, 0, mcu_rpm_max_));
} }
// 解析清扫开关 // 解析清扫开关
@ -135,9 +136,15 @@ class CanRadioCtrlNode : public rclcpp::Node
const char* gear_str = "未知挡位"; const char* gear_str = "未知挡位";
switch (msg.gear) switch (msg.gear)
{ {
case 0: gear_str = "空挡"; break; case 0:
case 1: gear_str = "后退挡"; break; gear_str = "空挡";
case 2: gear_str = "前进挡"; break; break;
case 1:
gear_str = "后退挡";
break;
case 2:
gear_str = "前进挡";
break;
} }
const char* sweep_str = msg.sweep ? "正在清扫" : "未清扫"; const char* sweep_str = msg.sweep ? "正在清扫" : "未清扫";
@ -150,19 +157,23 @@ class CanRadioCtrlNode : public rclcpp::Node
" 清扫状态: %s\n" " 清扫状态: %s\n"
"=====================================", "=====================================",
brake_str, gear_str, static_cast<int>(msg.rpm), msg.angle, sweep_str); brake_str, gear_str, static_cast<int>(msg.rpm), msg.angle, sweep_str);
print_counter = 0; print_counter = 0; // 重置计数器
} }
} }
else else
{ {
// 未接收到数据或数据不安全（不变） // 未接收到数据或数据不安全
int interval = PRINT_INTERVAL / 2; int interval = PRINT_INTERVAL / 2;
if (++print_counter >= interval) if (++print_counter >= interval)
{ {
if (!has_received) if (!has_received)
{
LOG_INFO("Waiting for CAN radio control data..."); LOG_INFO("Waiting for CAN radio control data...");
}
else else
{
LOG_WARN("CAN radio control unabled!"); LOG_WARN("CAN radio control unabled!");
}
print_counter = 0; print_counter = 0;
} }
return; return;