cxh/sweeper_e800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Auto commit at 2025-06-06 16:10:18

Browse Source
This commit is contained in:
cxh 2025-06-06 16:10:18 +08:00
parent 8d8e874de9
commit 9d3daf70fe
1 changed files with 30 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
src/radio_ctrl/src/radio_ctrl.cpp
View File

@ -8,6 +8,7 @@
constexpr float EPS_GEAR_RATIO = 16.5f;
constexpr float DELTA_T = 0.02f; // 20ms
const uint16_t speed[3] = {192, 992, 1792};
class SBUSNode : public rclcpp::Node
{
@ -71,9 +72,9 @@ private:
for (int i = 0; i < 10; ++i)
{
ch_data[i] = uart_handler_->get_channel_value(i);
printf("ch[%d]:%d ", i, ch_data[i]);
// printf("ch[%d]:%d ", i, ch_data[i]);
}
printf("\n");
// printf("\n");
if (ch_data[6] == 192) // 是否使能车辆控制
{
@ -98,17 +99,17 @@ private:
}
// 油门 / 刹车逻辑
if (ch_data[1] >= 1016)
if (ch_data[1] <= speed[1])
{
msg.brake = 1;
msg.brake_time_ms = static_cast<uint16_t>(30000 + (500 - 30000) * (ch_data[1] - 1016) / (1816 - 1016));
msg.brake_time_ms = static_cast<uint16_t>(30000 + (500 - 30000) * (speed[1] - ch_data[1]) / (speed[1] - speed[0]));
msg.rpm = 0;
}
else
{
msg.brake = 0;
msg.brake_time_ms = 500;
msg.rpm = static_cast<uint16_t>(MCU_RPM_MAX * (1016 - ch_data[1]) / (1016 - 216));
msg.rpm = static_cast<uint16_t>(MCU_RPM_MAX * (ch_data[1] - speed[1]) / (speed[2] - speed[1]));
}
// 一键清扫
@ -138,7 +139,7 @@ private:
}
// 转向逻辑
float target_angle = (static_cast<float>(ch_data[3]) - 800) / 800 * EPS_ANGLE_MAX;
float target_angle = (992 - static_cast<float>(ch_data[3])) / 800 * EPS_ANGLE_MAX;
// 角速度（度/秒）
float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T;
@ -177,29 +178,29 @@ private:
// 发布控制消息
pub_->publish(msg);
// RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:"
// << "\n mcu_enabled: " << msg.mcu_enabled
// << "\n brake: " << static_cast<int>(msg.brake)
// << "\n gear: " << static_cast<int>(msg.gear)
// << "\n rpm: " << msg.rpm
// << "\n brake_time_ms: " << msg.brake_time_ms
// << "\n angle: " << msg.angle
// << "\n angle_speed: " << msg.angle_speed
// << "\n main_brush_lift: " << msg.main_brush_lift
// << "\n edge_brush_lift: " << msg.edge_brush_lift
// << "\n vacuum: " << msg.vacuum
// << "\n spray: " << msg.spray
// << "\n mud_flap: " << msg.mud_flap
// << "\n dust_shake: " << msg.dust_shake
// << "\n left_light: " << msg.left_light
// << "\n right_light: " << msg.right_light
// << "\n night_light: " << msg.night_light
// << "\n brake_light: " << msg.brake_light
// << "\n headlight: " << msg.headlight
// << "\n main_brush_spin: " << msg.main_brush_spin
// << "\n edge_brush_spin: " << msg.edge_brush_spin
// << "\n main_brush_pwm: " << static_cast<int>(msg.main_brush_pwm)
// << "\n edge_brush_pwm: " << static_cast<int>(msg.edge_brush_pwm));
RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:"
<< "\n mcu_enabled: " << msg.mcu_enabled
<< "\n brake: " << static_cast<int>(msg.brake)
<< "\n gear: " << static_cast<int>(msg.gear)
<< "\n rpm: " << msg.rpm
<< "\n brake_time_ms: " << msg.brake_time_ms
<< "\n angle: " << msg.angle
<< "\n angle_speed: " << msg.angle_speed
<< "\n main_brush_lift: " << msg.main_brush_lift
<< "\n edge_brush_lift: " << msg.edge_brush_lift
<< "\n vacuum: " << msg.vacuum
<< "\n spray: " << msg.spray
<< "\n mud_flap: " << msg.mud_flap
<< "\n dust_shake: " << msg.dust_shake
<< "\n left_light: " << msg.left_light
<< "\n right_light: " << msg.right_light
<< "\n night_light: " << msg.night_light
<< "\n brake_light: " << msg.brake_light
<< "\n headlight: " << msg.headlight
<< "\n main_brush_spin: " << msg.main_brush_spin
<< "\n edge_brush_spin: " << msg.edge_brush_spin
<< "\n main_brush_pwm: " << static_cast<int>(msg.main_brush_pwm)
<< "\n edge_brush_pwm: " << static_cast<int>(msg.edge_brush_pwm));
}
else
{