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

Auto commit at 2025-05-27 08:42:33

Browse Source
This commit is contained in:
root 2025-05-27 08:42:33 +08:00
parent ab26100687
commit f967a1ef92
3 changed files with 106 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -1,4 +1,4 @@
/build/
/install/
/log/
/auto_git_push.sh

6
a.sh
View File

@ -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

205
src/radio_ctrl/src/radio_ctrl.cpp
View File

@ -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<uint16_t>(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<uint16_t>(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<uint16_t>(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<uint16_t>(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<float>(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<uint16_t>(motor_rpm),
static_cast<uint16_t>(120),
static_cast<uint16_t>(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<float>(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<uint16_t>(motor_rpm),
static_cast<uint16_t>(120),
static_cast<uint16_t>(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<int>(msg.brake)
@ -197,8 +200,10 @@ private:
<< "\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));
pub_->publish(msg); // 发布控制消息
}
else
{
return; // 下次循环，等待数据安全
}
}