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-02-25 13:39:57 +08:00
parent 09fa75aa90
commit 92beb100b8
2 changed files with 47 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,13 +1,13 @@
#include <algorithm> #include <algorithm>
#include <cmath> // 补充fabs所需头文件
#include <iostream> #include <iostream>
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
#include <cmath> // 补充fabs所需头文件
#include "can_radio_ctrl/candev.h" #include "can_radio_ctrl/candev.h"
#include "can_radio_ctrl/get_config.h" #include "can_radio_ctrl/get_config.h"
#include "logger/logger.h" #include "logger/logger.h"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include "sweeper_interfaces/msg/can_frame.hpp" #include "sweeper_interfaces/msg/can_frame.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
namespace sweeperMsg = sweeper_interfaces::msg; namespace sweeperMsg = sweeper_interfaces::msg;
@ -46,10 +46,8 @@ class CanRadioCtrlNode : public rclcpp::Node
"can_data", 10, std::bind(&CanRadioCtrlNode::can_callback, this, std::placeholders::_1)); "can_data", 10, std::bind(&CanRadioCtrlNode::can_callback, this, std::placeholders::_1));
// 创建周期定时器（每20ms回调一次控制逻辑，50Hz） // 创建周期定时器（每20ms回调一次控制逻辑，50Hz）
timer_ = this->create_wall_timer( timer_ =
std::chrono::milliseconds(20), this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&CanRadioCtrlNode::timer_callback, this));
std::bind(&CanRadioCtrlNode::timer_callback, this)
);
LOG_INFO("CanRadioCtrlNode initialized successfully!"); LOG_INFO("CanRadioCtrlNode initialized successfully!");
} }
@ -77,12 +75,21 @@ class CanRadioCtrlNode : public rclcpp::Node
if (has_received && data_safe) if (has_received && data_safe)
{ {
// 解析挡位 // 解析挡位
switch(radio_data.gear) switch (radio_data.gear) // 通道E
{ {
case 0: msg.gear = 1; break; // R挡 case 0:
case 1: msg.gear = 0; break; // N挡 msg.gear = 1;
case 2: msg.gear = 2; break; // D挡 break; // R挡
default: msg.gear = 0; LOG_WARN("Unknown gear: %d", radio_data.gear); break; 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;
} }
// 解析油门/刹车 // 解析油门/刹车
@ -100,7 +107,7 @@ class CanRadioCtrlNode : public rclcpp::Node
} }
// 解析清扫开关 // 解析清扫开关
msg.sweep = (radio_data.sweep != 0); msg.sweep = (radio_data.sweep != 0); // 通道B
// 解析转向 // 解析转向
float steering_ratio = static_cast<float>(radio_data.steering) / 450.0f; float steering_ratio = static_cast<float>(radio_data.steering) / 450.0f;
@ -114,9 +121,7 @@ class CanRadioCtrlNode : public rclcpp::Node
float motor_rpm = angle_speed * DEG_PER_SEC_TO_RPM * GEAR_RATIO; float motor_rpm = angle_speed * DEG_PER_SEC_TO_RPM * GEAR_RATIO;
// 限制电机转速到 [120, 1500] 范围 // 限制电机转速到 [120, 1500] 范围
uint16_t can_speed = static_cast<uint16_t>( uint16_t can_speed = static_cast<uint16_t>(std::clamp(motor_rpm, 120.0f, 1500.0f));
std::clamp(motor_rpm, 120.0f, 1500.0f)
);
msg.angle = target_angle; msg.angle = target_angle;
msg.angle_speed = can_speed; msg.angle_speed = can_speed;
@ -131,9 +136,15 @@ class CanRadioCtrlNode : public rclcpp::Node
const char* gear_str = "未知挡位"; const char* gear_str = "未知挡位";
switch (msg.gear) switch (msg.gear)
{ {
case 0: gear_str = "空挡"; break; case 0:
case 1: gear_str = "后退挡"; break; gear_str = "空挡";
case 2: gear_str = "前进挡"; break; break;
case 1:
gear_str = "后退挡";
break;
case 2:
gear_str = "前进挡";
break;
} }
const char* sweep_str = msg.sweep ? "正在清扫" : "未清扫"; const char* sweep_str = msg.sweep ? "正在清扫" : "未清扫";
@ -145,8 +156,7 @@ class CanRadioCtrlNode : public rclcpp::Node
" 轮端转向角度: %.1f °\n" " 轮端转向角度: %.1f °\n"
" 清扫状态: %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; // 重置计数器
} }
} }

18
src/control/can_radio_ctrl/src/candev.cpp
View File

@ -116,16 +116,18 @@ void can_recv_loop()
} }
else if (frame.can_id == 0x10A && frame.can_dlc >= 8) else if (frame.can_id == 0x10A && frame.can_dlc >= 8)
{ {
radio_data.gear = frame.data[0]; uint8_t ch1 = frame.data[0]; // 通道E
radio_data.sweep = frame.data[1]; uint8_t ch2 = frame.data[1]; // 通道F
uint8_t ch3 = frame.data[2]; uint8_t ch3 = frame.data[2]; // 通道A
uint8_t ch4 = frame.data[3]; uint8_t ch4 = frame.data[3]; // 通道B
uint8_t ch5 = frame.data[4]; uint8_t ch5 = frame.data[4]; // 通道C
uint8_t ch6 = frame.data[5]; uint8_t ch6 = frame.data[5]; // 通道D
uint8_t ch7 = frame.data[6]; uint8_t ch7 = frame.data[6];
uint8_t ch8 = frame.data[7]; uint8_t ch8 = frame.data[7];
printf("ch1 : %d ,ch2 : %d ,ch3 : %d ,ch4 : %d\n", radio_data.gear, radio_data.sweep, ch3, ch4); radio_data.gear = ch1;
printf("ch5 : %d ,ch6 : %d ,ch7 : %d ,ch8 : %d\n", ch5, ch6, ch7, ch8); radio_data.sweep = ch4;
// printf("ch1 : %d ,ch2 : %d ,ch3 : %d ,ch4 : %d\n", ch1, ch2, ch3, ch4);
// printf("ch5 : %d ,ch6 : %d ,ch7 : %d ,ch8 : %d\n", ch5, ch6, ch7, ch8);
radio_data.data_valid = true; radio_data.data_valid = true;
} }
} }