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 EPS_GEAR_RATIO = 16.5f;
constexpr float DELTA_T = 0.02f; // 20ms constexpr float DELTA_T = 0.02f; // 20ms
const uint16_t speed[3] = {192, 992, 1792};
class SBUSNode : public rclcpp::Node class SBUSNode : public rclcpp::Node
{ {
@ -71,9 +72,9 @@ private:
for (int i = 0; i < 10; ++i) for (int i = 0; i < 10; ++i)
{ {
ch_data[i] = uart_handler_->get_channel_value(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) // 是否使能车辆控制 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 = 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; msg.rpm = 0;
} }
else else
{ {
msg.brake = 0; msg.brake = 0;
msg.brake_time_ms = 500; 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; float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T;
@ -177,29 +178,29 @@ private:
// 发布控制消息 // 发布控制消息
pub_->publish(msg); pub_->publish(msg);
// RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:"
// << "\n mcu_enabled: " << msg.mcu_enabled << "\n mcu_enabled: " << msg.mcu_enabled
// << "\n brake: " << static_cast<int>(msg.brake) << "\n brake: " << static_cast<int>(msg.brake)
// << "\n gear: " << static_cast<int>(msg.gear) << "\n gear: " << static_cast<int>(msg.gear)
// << "\n rpm: " << msg.rpm << "\n rpm: " << msg.rpm
// << "\n brake_time_ms: " << msg.brake_time_ms << "\n brake_time_ms: " << msg.brake_time_ms
// << "\n angle: " << msg.angle << "\n angle: " << msg.angle
// << "\n angle_speed: " << msg.angle_speed << "\n angle_speed: " << msg.angle_speed
// << "\n main_brush_lift: " << msg.main_brush_lift << "\n main_brush_lift: " << msg.main_brush_lift
// << "\n edge_brush_lift: " << msg.edge_brush_lift << "\n edge_brush_lift: " << msg.edge_brush_lift
// << "\n vacuum: " << msg.vacuum << "\n vacuum: " << msg.vacuum
// << "\n spray: " << msg.spray << "\n spray: " << msg.spray
// << "\n mud_flap: " << msg.mud_flap << "\n mud_flap: " << msg.mud_flap
// << "\n dust_shake: " << msg.dust_shake << "\n dust_shake: " << msg.dust_shake
// << "\n left_light: " << msg.left_light << "\n left_light: " << msg.left_light
// << "\n right_light: " << msg.right_light << "\n right_light: " << msg.right_light
// << "\n night_light: " << msg.night_light << "\n night_light: " << msg.night_light
// << "\n brake_light: " << msg.brake_light << "\n brake_light: " << msg.brake_light
// << "\n headlight: " << msg.headlight << "\n headlight: " << msg.headlight
// << "\n main_brush_spin: " << msg.main_brush_spin << "\n main_brush_spin: " << msg.main_brush_spin
// << "\n edge_brush_spin: " << msg.edge_brush_spin << "\n edge_brush_spin: " << msg.edge_brush_spin
// << "\n main_brush_pwm: " << static_cast<int>(msg.main_brush_pwm) << "\n main_brush_pwm: " << static_cast<int>(msg.main_brush_pwm)
// << "\n edge_brush_pwm: " << static_cast<int>(msg.edge_brush_pwm)); << "\n edge_brush_pwm: " << static_cast<int>(msg.edge_brush_pwm));
} }
else else
{ {