diff --git a/.gitignore b/.gitignore index 7f207df..5429c62 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ /build/ /install/ /log/ - +/auto_git_push.sh diff --git a/a.sh b/a.sh deleted file mode 100755 index eb57d95..0000000 --- a/a.sh +++ /dev/null @@ -1,6 +0,0 @@ -sudo ip link set can0 down -sudo ip link set can0 up type can bitrate 500000 -sudo chmod 666 /dev/ttyUSB5 -sudo chmod 666 /dev/ttyUSB5 -source install/setup.bash -#sudo insmod drivers/usb/serial/pl2303.ko diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index 89cea5d..c298d50 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/src/radio_ctrl/src/radio_ctrl.cpp @@ -6,6 +6,9 @@ #include "mc/msg/mc_ctrl.hpp" #include "mc/msg/can_frame.hpp" +constexpr float EPS_GEAR_RATIO = 16.5f; +constexpr float DELTA_T = 0.02f; // 20ms + class SBUSNode : public rclcpp::Node { public: @@ -54,63 +57,113 @@ private: // 定时器回调函数，用于周期性发布控制指令 void timer_callback() { - int MCU_RPM_MAX = config.mcu_rpm_max; - float EPS_ANGLE_MAX = config.eps_angle_max; - constexpr float EPS_GEAR_RATIO = 16.5f; - constexpr float DELTA_T = 0.02f; // 20ms + static int MCU_RPM_MAX = config.mcu_rpm_max; + static float EPS_ANGLE_MAX = config.eps_angle_max; + + bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性 auto msg = mc::msg::McCtrl(); // 控制消息对象 + uint16_t ch_data[8]; // 各通道遥控数据 - // 获取各通道遥控数据 - uint16_t ch_data[8]; - // 获取数据安全性 - bool data_safe = uart_handler_->get_data_safe(); - for (int i = 0; i < 8; ++i) + if (data_safe) // 数据安全，进行数据解析并发布 { - ch_data[i] = uart_handler_->get_channel_value(i); - printf("ch[%d]:%d ", i, ch_data[i]); - } - printf("\n"); - // 是否使能车辆控制 - if (ch_data[6] == 192) - { - msg.mcu_enabled = true; - msg.brake = 0; - // 挡位选择 - if (ch_data[7] == 192) + // 赋值与打印 + for (int i = 0; i < 8; ++i) { - msg.gear = 3; // D挡 - } - else if (ch_data[7] == 1792) - { - msg.gear = 1; // R挡 - } - else if (ch_data[7] == 992) - { - msg.gear = 2; // N挡 - } - else - { - msg.gear = 0; // 未知状态 + ch_data[i] = uart_handler_->get_channel_value(i); + printf("ch[%d]:%d ", i, ch_data[i]); } + printf("\n"); - // 油门 / 刹车逻辑 - if (ch_data[1] >= 1016) - { - msg.brake = 1; - msg.brake_time_ms = static_cast(30000 + (500 - 30000) * (ch_data[1] - 1016) / (1816 - 1016)); - msg.rpm = 0; - } - else + if (ch_data[6] == 192) // 是否使能车辆控制 { + msg.mcu_enabled = true; msg.brake = 0; - msg.brake_time_ms = 500; - msg.rpm = static_cast(MCU_RPM_MAX * (1016 - ch_data[1]) / (1016 - 216)); - } + // 挡位选择 + if (ch_data[7] == 192) + { + msg.gear = 3; // D挡 + } + else if (ch_data[7] == 1792) + { + msg.gear = 1; // R挡 + } + else if (ch_data[7] == 992) + { + msg.gear = 2; // N挡 + } + else + { + msg.gear = 0; // 未知状态 + } - // 一键清扫 - if (ch_data[5] == 1792) + // 油门 / 刹车逻辑 + if (ch_data[1] >= 1016) + { + msg.brake = 1; + msg.brake_time_ms = static_cast(30000 + (500 - 30000) * (ch_data[1] - 1016) / (1816 - 1016)); + msg.rpm = 0; + } + else + { + msg.brake = 0; + msg.brake_time_ms = 500; + msg.rpm = static_cast(MCU_RPM_MAX * (1016 - ch_data[1]) / (1016 - 216)); + } + + // 一键清扫 + if (ch_data[5] == 1792) + { + msg.main_brush_lift = false; + msg.edge_brush_lift = false; + msg.vacuum = false; + msg.spray = false; + msg.mud_flap = false; + msg.dust_shake = false; + msg.main_brush_spin = false; + msg.edge_brush_spin = false; + } + else if (ch_data[5] == 192) + { + msg.main_brush_lift = true; + msg.edge_brush_lift = true; + msg.vacuum = true; + msg.spray = true; + msg.mud_flap = true; + msg.dust_shake = false; + msg.main_brush_spin = true; + msg.edge_brush_spin = true; + msg.main_brush_pwm = 100; + msg.edge_brush_pwm = 100; + } + + // 转向逻辑 + float target_angle = (static_cast(ch_data[3]) - 800) / 800 * EPS_ANGLE_MAX; + + // 角速度（度/秒） + float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T; + + // 电机转速（单位：rpm） + float motor_rpm = angle_speed * EPS_GEAR_RATIO; + + // 限制电机转速到 [120, 1500] 范围，防止过小/过大 + uint16_t can_speed = std::clamp( + static_cast(motor_rpm), + static_cast(120), + static_cast(1500)); + + msg.angle = target_angle; + msg.angle_speed = can_speed; + } + else // 未使能状态，发送安全默认控制指令 { + msg.mcu_enabled = false; + msg.brake = 1; + msg.gear = 0; + msg.rpm = 0; + msg.brake_time_ms = 500; + msg.angle = 0; + msg.angle_speed = 120; msg.main_brush_lift = false; msg.edge_brush_lift = false; msg.vacuum = false; @@ -120,60 +173,10 @@ private: msg.main_brush_spin = false; msg.edge_brush_spin = false; } - else if (ch_data[5] == 192) - { - msg.main_brush_lift = true; - msg.edge_brush_lift = true; - msg.vacuum = true; - msg.spray = true; - msg.mud_flap = true; - msg.dust_shake = false; - msg.main_brush_spin = true; - msg.edge_brush_spin = true; - msg.main_brush_pwm = 100; - msg.edge_brush_pwm = 100; - } - // 转向逻辑 - float target_angle = (static_cast(ch_data[3]) - 800) / 800 * EPS_ANGLE_MAX; + // 发布控制消息 + pub_->publish(msg); - // 角速度（度/秒） - float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T; - - // 电机转速（单位：rpm） - float motor_rpm = angle_speed * EPS_GEAR_RATIO; - - // 限制电机转速到 [120, 1500] 范围，防止过小/过大 - uint16_t can_speed = std::clamp( - static_cast(motor_rpm), - static_cast(120), - static_cast(1500)); - - msg.angle = target_angle; - msg.angle_speed = can_speed; - } - else - { - // 未使能状态，发送安全默认控制指令 - msg.mcu_enabled = false; - msg.brake = 1; - msg.gear = 0; - msg.rpm = 0; - msg.brake_time_ms = 500; - msg.angle = 0; - msg.angle_speed = 120; - msg.main_brush_lift = false; - msg.edge_brush_lift = false; - msg.vacuum = false; - msg.spray = false; - msg.mud_flap = false; - msg.dust_shake = false; - msg.main_brush_spin = false; - msg.edge_brush_spin = false; - } - - if (data_safe) - { RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" << "\n mcu_enabled: " << msg.mcu_enabled << "\n brake: " << static_cast(msg.brake) @@ -197,8 +200,10 @@ private: << "\n edge_brush_spin: " << msg.edge_brush_spin << "\n main_brush_pwm: " << static_cast(msg.main_brush_pwm) << "\n edge_brush_pwm: " << static_cast(msg.edge_brush_pwm)); - - pub_->publish(msg); // 发布控制消息 + } + else + { + return; // 下次循环，等待数据安全 } }