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

不解析档位，仅用油门控制进退

Browse Source
This commit is contained in:
lyq 2026-03-30 10:13:07 +08:00
parent 27829447c0
commit e75370756b
1 changed files with 29 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -74,37 +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
{
case 0:
msg.gear = 1;
break; // R挡
case 1:
msg.gear = 0;
break; // N挡
case 2:
msg.gear = 2;
break; // D挡
default:
msg.gear = 0;
LOG_WARN("Unknown gear: %d", radio_data.gear);
break;
}
// 解析油门/刹车
if (radio_data.throttle <= 0)
{
msg.brake = 1;
msg.rpm = 0;
}
else
{ {
// 油门 > 0 → 前进挡 D
msg.gear = 2;
msg.brake = 0; msg.brake = 0;
// 计算前进转速
float throttle_ratio = static_cast<float>(radio_data.throttle) / 450.0f; float throttle_ratio = static_cast<float>(radio_data.throttle) / 450.0f;
int rpm_val = static_cast<int>(mcu_rpm_max_ * throttle_ratio); int rpm_val = static_cast<int>(mcu_rpm_max_ * throttle_ratio);
msg.rpm = static_cast<uint16_t>(std::clamp(rpm_val, 0, mcu_rpm_max_)); msg.rpm = static_cast<uint16_t>(std::clamp(rpm_val, 0, mcu_rpm_max_));
} }
else if (radio_data.throttle < 0)
{
// 油门 < 0 → 后退挡 R
msg.gear = 1;
msg.brake = 0;
// 取绝对值计算后退转速（后退速度和前进一致）
float throttle_abs = fabs(radio_data.throttle);
float throttle_ratio = static_cast<float>(throttle_abs) / 450.0f;
int rpm_val = static_cast<int>(mcu_rpm_max_ * throttle_ratio);
msg.rpm = static_cast<uint16_t>(std::clamp(rpm_val, 0, mcu_rpm_max_));
}
else
{
// 油门 = 0 → 空挡 + 刹车
msg.gear = 0;
msg.brake = 1;
msg.rpm = 0;
}
// 解析清扫开关 // 解析清扫开关
msg.sweep = (radio_data.sweep != 0); // 通道B msg.sweep = (radio_data.sweep != 0); // 通道B
@ -136,15 +135,9 @@ class CanRadioCtrlNode : public rclcpp::Node
const char* gear_str = "未知挡位"; const char* gear_str = "未知挡位";
switch (msg.gear) switch (msg.gear)
{ {
case 0: case 0: gear_str = "空挡"; break;
gear_str = "空挡"; case 1: gear_str = "后退挡"; break;
break; case 2: gear_str = "前进挡"; break;
case 1:
gear_str = "后退挡";
break;
case 2:
gear_str = "前进挡";
break;
} }
const char* sweep_str = msg.sweep ? "正在清扫" : "未清扫"; const char* sweep_str = msg.sweep ? "正在清扫" : "未清扫";
@ -157,23 +150,19 @@ 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;