HiveCoreRD/hivecore_robot_motion
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

new leg configuration for R1 robot

Browse Source
This commit is contained in:
NuoDaJia02
2025-12-23 15:20:03 +08:00
parent 7d892a6c98
commit b0612c3de8
8 changed files with 161 additions and 531 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/robot_control/include/leg_control.hpp
View File

@@ -6,7 +6,6 @@
namespace robot_control
{
/**
* @brief LegControl 类用于控制腿部运动
*
@@ -31,6 +30,8 @@ namespace robot_control
bool SetMoveLegParametersInternal(double moveDistance);
double calculate_angle_from_links(double side1, double side2, double side_opposite, bool is_degree = false);
bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
bool MoveUp(std::vector<double>& joint_positions, double dt);

84
src/robot_control/include/motion_parameters.hpp
View File

@@ -65,8 +65,11 @@ namespace robot_control
// 腿长参数 unit m
leg_length_ = {
0.70,
0.66
0.66,
0.385,
0.362,
0.55,
0.4
};
waist_length_ = {
@@ -85,6 +88,8 @@ namespace robot_control
50,
50,
50,
50,
50,
50
};
@@ -94,43 +99,43 @@ namespace robot_control
100,
100,
100,
100,
100,
100
};
// 初始化关节索引
wheel_joint_indices_ = {0, 1};
waist_joint_indices_ = {0};
waist_joint_indices_ = {0, 1};
leg_joint_indices_ = {2, 3, 4, 5};
leg_joint_indices_ = {2, 3, 4, 5, 6, 7};
total_joints_ = 1;
total_joints_ = 8;
// 初始化关节方向
wheel_joint_directions_ = {1, -1};
waist_joint_directions_ = {1};
waist_joint_directions_ = {1, 1};
leg_joint_directions_ = {1, -1, -1, 1};
leg_joint_directions_ = {-1, 1, 1, -1, -1, 1};
//TODO: check the ratio
joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0}; // 传动比 都一样
joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}; // 传动比 都一样
joint_resolution_ = {524288, 131072, 131072 , 131072, 131072, 131072}; // 分辨率 都一样
joint_resolution_ = {524288, 524288, 524288 , 524288, 524288, 524288, 524288, 524288}; // 分辨率 都一样
//TODO: 限位需要修改
joint_limits_ = {
JointLimit(0, 40.0, -40.0, LimitType::POSITION),
JointLimit(1, 190.0, -190.0, LimitType::POSITION),
JointLimit(2, 80.0, 0.0, LimitType::POSITION),
// JointLimit(3, 5.0, -5.0, LimitType::POSITION),
// JointLimit(4, 50.0, -50.0, LimitType::POSITION),
JointLimit(3, 0.0, -60.0, LimitType::POSITION),
JointLimit(4, 0, -80.0, LimitType::POSITION),
// JointLimit(7, 5.0, -5.0, LimitType::POSITION),
// JointLimit(8, 50.0, -50.0, LimitType::POSITION),
JointLimit(5, 60.0, 0.0, LimitType::POSITION),
JointLimit(2, 0.0, -90.0, LimitType::POSITION),
JointLimit(3, 90.0, 0.0, LimitType::POSITION),
JointLimit(4, 60, 0.0, LimitType::POSITION),
JointLimit(5, 0.0, -90.0, LimitType::POSITION),
JointLimit(6, 0.0, -60.0, LimitType::POSITION),
JointLimit(7, 90.0, 0.0, LimitType::POSITION),
};
// 初始化限制参数
@@ -185,24 +190,22 @@ namespace robot_control
}
waist_zero_positions_ = {
181.0
// 52.66
350.0,
261.82
};
leg_zero_positions_ = {
-18.36,
// 129.71,
// 45.02,
25.54,
-21.54,
// 167.96,
// 16.26,
-127.24
217.52, //316786.46
120.84, //175986.005
108.7, //158305.849
221.95, //323238.116
234.14, //340991.09
125.39 //182612.423
};
waist_home_positions_ = {
181.0
// 52.66
350.0,
261.82
};
@@ -210,16 +213,23 @@ namespace robot_control
// 前腿零点为站直时候的状态, 然后 髋部 pitch + 49度, 大腿长 0.38m, 38度为 0.25m , 小腿长0.36 膝盖 pitch 再收回 35.5 度, 剩余 13.5度 长度为 0.35m
// 初始化初始位置
leg_home_positions_ = {
30.64,
// 129.71,
// 9.52,
0.54,
-70.54,
// 167.96,
// 51.76,
-102.24
217.52 - 65.0,
120.84 + 41.0,
108.7 + 35.5, //217479
221.95 - 41.0,
234.14 - 35.5, //298023
125.39 + 65.0
};
// leg_home_positions_ = {
// 217.52 - 90.0, //185.714.46
// 120.84 + 90.0, //307058
// 108.7 + 0.0, //158305
// 221.95 - 90.0, //192166
// 234.14 - 0.0, //340991
// 125.39 + 90.0 //313684
// };
// 初始化零点位置
wheel_home_positions_ = {
0.0,

2
src/robot_control/include/robot_control_node.hpp
View File

@@ -18,6 +18,8 @@
#include "interfaces/msg/motor_cmd.hpp"
#include "interfaces/srv/motor_param.hpp"
#define RECORD_FLAG 0
using MoveHome = interfaces::action::MoveHome;
using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;

94
src/robot_control/src/leg_control.cpp
View File

@@ -75,48 +75,97 @@ bool LegControl::MoveUp(std::vector<double>& joint_positions, double dt)
return result;
}
double LegControl::calculate_angle_from_links(double side1, double side2, double side_opposite, bool is_degree)
{
const double EPS = 1e-6; // 浮点精度容错值
if (side1 < EPS || side2 < EPS || side_opposite < EPS) {
throw std::invalid_argument("连杆长度必须大于0！");
}
// 2. 检查是否能构成三角形（任意两边之和大于第三边，容错EPS）
if ((side1 + side2 - side_opposite) < EPS ||
(side1 + side_opposite - side2) < EPS ||
(side2 + side_opposite - side1) < EPS) {
throw std::invalid_argument("三边长度无法构成三角形（任意两边之和需大于第三边）！");
}
// 3. 余弦定理计算夹角（弧度）
double numerator = side1 * side1 + side2 * side2 - side_opposite * side_opposite;
double denominator = 2 * side1 * side2;
double cos_theta = numerator / denominator;
// 4. 处理浮点精度误差：确保cos_theta在[-1, 1]范围内（避免acos定义域错误）
cos_theta = std::max(-1.0, std::min(1.0, cos_theta));
// 5. 计算弧度值
double angle_rad = acos(cos_theta);
// 6. 按需转换为角度
if (is_degree) {
return angle_rad * 180.0 / M_PI;
}
return angle_rad;
}
bool LegControl::SetMoveLegParametersInternal(double moveDistance)
{
std::vector<double> tempPosition;
tempPosition.resize(total_joints_, 0.0);
double currentFrountLegLength = lengthParameters_[0] * abs(std::cos(DEG2RAD(joint_position_[0] - joint_zero_positions_[0])));
double backJoint1 = DEG2RAD(joint_position_[0] - joint_zero_positions_[0]);
double frountJoint1 = DEG2RAD(joint_position_[1] - joint_zero_positions_[1]);
double frountJoint2 = DEG2RAD(joint_position_[2] - joint_zero_positions_[2]);
double currentBackLegLength = lengthParameters_[1] * abs(std::cos(DEG2RAD(joint_position_[1] - joint_zero_positions_[1])));
double currentBackLegHeight = lengthParameters_[0] * abs(std::sin(backJoint1));
double currentFrountLegHeight = lengthParameters_[1] * abs(std::sin(frountJoint1)) + lengthParameters_[2] * abs(std::sin(frountJoint1 + frountJoint2));
double currentFrountLegDistance = lengthParameters_[1] * abs(std::cos(frountJoint1)) + lengthParameters_[2] * abs(std::cos(frountJoint1 + frountJoint2));
if (moveDistance > 0)
{
if ((moveDistance + currentFrountLegLength) > (lengthParameters_[0] - 0.02) || (moveDistance + currentBackLegLength) > (lengthParameters_[1] - 0.03))
if ((moveDistance + currentFrountLegHeight) > lengthParameters_[3])
{
std::cout << "Move distance is too large! " << std::endl;
std::cout << " Current Front Leg Length: " << currentFrountLegLength << std::endl;
std::cout << " Current Back Leg Length: " << currentBackLegLength << std::endl;
std::cout << " Current Front Leg Length: " << currentFrountLegHeight << std::endl;
std::cout << " Current Back Leg Length: " << currentBackLegHeight << std::endl;
std::cout << " Move Distance: " << moveDistance << std::endl;
return false;
}
}
else
{
if (moveDistance < (-currentFrountLegLength + 0.03) || moveDistance < (-currentBackLegLength + 0.03))
if (moveDistance + currentFrountLegHeight < lengthParameters_[4])
{
std::cout << "Move distance is too large!" << std::endl;
std::cout << " Current Front Leg Length: " << currentFrountLegLength << std::endl;
std::cout << " Current Back Leg Length: " << currentBackLegLength << std::endl;
std::cout << " Current Front Leg Length: " << currentFrountLegHeight << std::endl;
std::cout << " Current Back Leg Length: " << currentBackLegHeight << std::endl;
std::cout << " Move Distance: " << moveDistance << std::endl;
return false;
}
}
double finalFrountLegLength = currentFrountLegLength + moveDistance;
double finalBackLegLength = currentBackLegLength + moveDistance;
double finalFrountLegHeight = currentFrountLegHeight + moveDistance;
double finalBackLegHeight = currentBackLegHeight + moveDistance;
double frontFinalAngle = RAD2DEG(std::acos(finalFrountLegLength / lengthParameters_[0]));
double backFinalAngle = RAD2DEG(std::acos(finalBackLegLength / lengthParameters_[1]));
double backFinalAngle = RAD2DEG(std::acos(finalBackLegHeight / lengthParameters_[0]));
tempPosition[0] = joint_directions_[0] * frontFinalAngle;
tempPosition[1] = joint_directions_[1] * backFinalAngle;
tempPosition[2] = joint_directions_[2] * frontFinalAngle;
tempPosition[3] = joint_directions_[3] * backFinalAngle;
double tempFrountLegLength = std::sqrt(finalFrountLegHeight * finalFrountLegHeight + currentFrountLegDistance * currentFrountLegDistance);
double frountAngle1_part1 = calculate_angle_from_links(finalFrountLegHeight, tempFrountLegLength, currentFrountLegDistance, true);
double frountAngle1_part2 = calculate_angle_from_links(tempFrountLegLength, lengthParameters_[1], lengthParameters_[2], true);
std::cout << "frountAngle1_part1 : " << frountAngle1_part1 << std::endl;
std::cout << "frountAngle1_part2 : " << frountAngle1_part2 << std::endl;
double frountFinalAngle1 = frountAngle1_part1 + frountAngle1_part2;
double frountFinalAngle2 = calculate_angle_from_links(lengthParameters_[1], lengthParameters_[2], tempFrountLegLength, true);
tempPosition[0] = joint_directions_[0] * backFinalAngle;
tempPosition[1] = joint_directions_[1] * (90.0 - frountFinalAngle1); //零点为水平方向
tempPosition[2] = joint_directions_[2] * (180.0 - frountFinalAngle2);
tempPosition[3] = joint_directions_[3] * (90.0 - frountFinalAngle1);
tempPosition[4] = joint_directions_[4] * (180.0 - frountFinalAngle2);
tempPosition[5] = joint_directions_[5] * backFinalAngle;
if (!checkJointLimits(tempPosition))
{
@@ -129,10 +178,15 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
joint_position_desired_[i] = tempPosition[i] + joint_zero_positions_[i];
}
std::cout << "Target Joint Position: "
<< joint_position_desired_[0]
<< ", " << joint_position_desired_[1] << ", "
<< joint_position_desired_[2] << ", " << joint_position_desired_[3] << std::endl;
std::cout << "Target relative joint Position: "
<< tempPosition[0] << ", " << tempPosition[1] << ", "
<< tempPosition[2] << ", " << tempPosition[3] << ", "
<< tempPosition[4] << ", " << tempPosition[5] << std::endl;
std::cout << "Target abs Joint Position: "
<< joint_position_desired_[0] << ", " << joint_position_desired_[1] << ", "
<< joint_position_desired_[2] << ", " << joint_position_desired_[3] << ", "
<< joint_position_desired_[4] << ", " << joint_position_desired_[5] << std::endl;
is_target_set_ = true;

67
src/robot_control/src/robot_control_manager.cpp
View File

@@ -405,24 +405,24 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
waistController_->UpdateJointStates(waistPositions, waistVelocities, waistEfforts);
// Update the leg controller
// size_t legStartIndex = motionParams_.leg_joint_indices_[0];
// size_t legJointSize = motionParams_.leg_joint_indices_.size();
size_t legStartIndex = motionParams_.leg_joint_indices_[0];
size_t legJointSize = motionParams_.leg_joint_indices_.size();
// std::vector<double> legPositions(
// jointPositions_.begin() + legStartIndex,
// jointPositions_.begin() + legStartIndex + legJointSize
// );
std::vector<double> legPositions(
jointPositions_.begin() + legStartIndex,
jointPositions_.begin() + legStartIndex + legJointSize
);
// std::vector<double> legVelocities(
// jointVelocities_.begin() + legStartIndex,
// jointVelocities_.begin() + legStartIndex + legJointSize
// );
std::vector<double> legVelocities(
jointVelocities_.begin() + legStartIndex,
jointVelocities_.begin() + legStartIndex + legJointSize
);
// std::vector<double> legEfforts(
// jointEfforts_.begin() + legStartIndex,
// jointEfforts_.begin() + legStartIndex + legJointSize
// );
// legController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
std::vector<double> legEfforts(
jointEfforts_.begin() + legStartIndex,
jointEfforts_.begin() + legStartIndex + legJointSize
);
legController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
}
MotionParameters RobotControlManager::GetMotionParameters()
@@ -437,11 +437,11 @@ bool RobotControlManager::IsMoving(MovementPart part)
case MovementPart::WHEEL:
return wheelController_->IsMoving();
case MovementPart::LEG:
return true; // legController_->IsMoving();
return legController_->IsMoving();
case MovementPart::WAIST:
return waistController_->IsMoving();
case MovementPart::ALL:
return wheelController_->IsMoving() && waistController_->IsMoving() ; //&& legController_->IsMoving();
return wheelController_->IsMoving() && waistController_->IsMoving() && legController_->IsMoving();
default:
return false;
}
@@ -457,12 +457,12 @@ bool RobotControlManager::Stop(double dt)
AssignTempValues();
bool waistStopped = waistController_->Stop(tempWaistCmd_, dt);
// bool legStopped = legController_->Stop(tempLegCmd_, dt);
bool legStopped = legController_->Stop(tempLegCmd_, dt);
bool wheelStopped = wheelController_->Stop(tempWheelCmd_, dt);
UpdateJointCommands();
return waistStopped && wheelStopped; //legStopped &&
return waistStopped && legStopped && wheelStopped;
}
bool RobotControlManager::MoveLeg(double dt)
@@ -496,7 +496,6 @@ bool RobotControlManager::MoveWheel()
for (size_t i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
{
wheelCommands_.data[motionParams_.wheel_joint_indices_[i]] = tempWheelCmd_[i];
// std::cout << "Wheel commands: " << i << " " << wheelCommands_.data[motionParams_.wheel_joint_indices_[i]] << std::endl;
}
return result;
@@ -512,12 +511,10 @@ bool RobotControlManager::GoHome(double dt)
isWaistHomed_ = waistController_->GoHome(tempWaistCmd_, dt);
}
isLegHomed_ = true;
// if (!isLegHomed_)
// {
// isLegHomed_ = legController_->GoHome(tempLegCmd_, dt);
// }
if (!isLegHomed_)
{
isLegHomed_ = legController_->GoHome(tempLegCmd_, dt);
}
UpdateJointCommands();
@@ -552,12 +549,12 @@ void RobotControlManager::AssignTempValues()
tempWaistCmd_[i] = jointCommands_.data[motionParams_.waist_joint_indices_[i]];
}
// tempLegCmd_.resize(motionParams_.leg_joint_indices_.size());
tempLegCmd_.resize(motionParams_.leg_joint_indices_.size());
// for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
// {
// tempLegCmd_[i] = jointCommands_.data[motionParams_.leg_joint_indices_[i]];
// }
for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
{
tempLegCmd_[i] = jointCommands_.data[motionParams_.leg_joint_indices_[i]];
}
}
void RobotControlManager::UpdateJointCommands()
@@ -567,8 +564,8 @@ void RobotControlManager::UpdateJointCommands()
jointCommands_.data[motionParams_.waist_joint_indices_[i]] = tempWaistCmd_[i];
}
// for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
// {
// jointCommands_.data[motionParams_.leg_joint_indices_[i]] = tempLegCmd_[i];
// }
for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.leg_joint_indices_[i]] = tempLegCmd_[i];
}
}

15
src/robot_control/src/robot_control_node.cpp
View File

@@ -20,7 +20,7 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
lastSpeed_ = 51;
// 初始化数据文件（设置路径，确保目录存在）
#if 0
#if RECORD_FLAG
data_file_path_ = "/home/demo/ros2_joint_data.txt"; // 数据保存路径
fs::path file_path(data_file_path_);
fs::create_directories(file_path.parent_path()); // 自动创建父目录（如果不存在）
@@ -456,17 +456,8 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
if((goal->move_distance > 0.1 ) && !robotControlManager_.GetJogWheel()) // || goal->move_distance < -0.5
{
// if (lastSpeed_ < 51 && goal->move_distance < -0.5)
// {
// ratio = 1.02;
// }
request->motor_id = 1;
request->max_speed = static_cast<uint16_t>(round((ratio) * 51));
// if (goal->move_distance < -0.5)
// {
// request->add_acc = 4;
// }
request->add_acc = 8;
request->dec_acc = 8;
@@ -524,7 +515,7 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
loop_rate.sleep();
}
if ((goal->move_distance > 0.0) && !robotControlManager_.GetJogWheel()) //|| goal->move_distance < -0.5
if ((goal->move_distance > 0.0) && !robotControlManager_.GetJogWheel())
{
request->motor_id = 1;
request->max_speed = 51;
@@ -611,7 +602,7 @@ void RobotControlNode::Publish_joint_trajectory()
cmd_msg = robotControlManager_.GetJointCommands();
}
#if 0
#if RECORD_FLAG
data_file_ << 0;
// 2. 保存整个数组数据到txt文件

419
src/robot_control/src/rs485_driver.cpp
View File

@@ -1,419 +0,0 @@
#include "rs485_driver.hpp"
#include <fcntl.h>
#include <unistd.h>
#include <cstring>
#include <iostream>
#include <chrono>
#include <thread>
namespace robot_control
{
const unsigned char CRC8Table[]={
0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,
190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,
70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,
219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,
101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,
248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,
140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,
17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,
175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,
50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,
202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,
87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,
233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53
};
/**
* @name CRC8_Table
* @brief This function is used to Get CRC8 check value.
* @param p :Array pointer. counter :Number of array elements.
* @retval crc8
*/
unsigned char CRC8_Table(unsigned char *p, int counter)
{
unsigned char crc8 = 0;
for( ; counter > 0; counter--)
{
crc8 = CRC8Table[crc8^*p];
p++;
}
return(crc8);
}
RS485Driver::RS485Driver(size_t maxMotors) : max_motors_(maxMotors), serial_port_(-1), is_open_(false)
{
control_index_ = 0;
}
RS485Driver::~RS485Driver()
{
closePort();
}
speed_t RS485Driver::convertBaudRate(int baud_rate)
{
switch (baud_rate)
{
case 9600: return B9600;
case 19200: return B19200;
case 38400: return B38400;
case 57600: return B57600;
case 115200: return B115200;
case 230400: return B230400;
case 460800: return B460800;
case 500000: return B500000;
case 576000: return B576000;
case 921600: return B921600;
default: return B0;
}
}
bool RS485Driver::openPort(const std::string &port_name, int baud_rate)
{
// 关闭已打开的端口
if (is_open_)
{
closePort();
}
// 打开串口
serial_port_ = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
if (serial_port_ == -1)
{
std::cerr << "无法打开串口: " << port_name << std::endl;
return false;
}
// 保存原始配置
if (tcgetattr(serial_port_, &original_termios_) == -1)
{
std::cerr << "无法获取串口属性" << std::endl;
close(serial_port_);
serial_port_ = -1;
return false;
}
// 配置串口
struct termios tty;
memset(&tty, 0, sizeof(tty));
// 设置波特率
speed_t speed = convertBaudRate(baud_rate);
if (speed == B0)
{
std::cerr << "不支持的波特率: " << baud_rate << std::endl;
closePort();
return false;
}
cfsetospeed(&tty, speed);
cfsetispeed(&tty, speed);
// 8位数据位，无奇偶校验，1位停止位
tty.c_cflag &= ~PARENB; // 无奇偶校验
tty.c_cflag &= ~CSTOPB; // 1位停止位
tty.c_cflag &= ~CSIZE; // 清除数据位设置
tty.c_cflag |= CS8; // 8位数据位
// 禁用硬件流控制
tty.c_cflag &= ~CRTSCTS;
// 启用接收器，设置本地模式
tty.c_cflag |= (CLOCAL | CREAD);
// 禁用软件流控制
tty.c_iflag &= ~(IXON | IXOFF | IXANY);
// 原始输入模式
tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
// 原始输出模式
tty.c_oflag &= ~OPOST;
// 设置读取超时
tty.c_cc[VTIME] = 10; // 1秒超时
tty.c_cc[VMIN] = 0;
// 应用配置
if (tcsetattr(serial_port_, TCSANOW, &tty) != 0)
{
std::cerr << "无法设置串口属性" << std::endl;
closePort();
return false;
}
is_open_ = true;
std::cout << "串口 " << port_name << " 已打开，波特率: " << baud_rate << std::endl;
return true;
}
void RS485Driver::closePort()
{
if (is_open_)
{
// 恢复原始配置
tcsetattr(serial_port_, TCSANOW, &original_termios_);
close(serial_port_);
is_open_ = false;
serial_port_ = -1;
std::cout << "串口已关闭" << std::endl;
}
}
bool RS485Driver::sendData(const std::vector<uint8_t> &data)
{
if (!is_open_)
{
std::cerr << "串口未打开，无法发送数据" << std::endl;
return false;
}
ssize_t bytes_written = write(serial_port_, data.data(), data.size());
if (bytes_written != static_cast<ssize_t>(data.size()))
{
std::cerr << "发送数据失败，预期发送 " << data.size()
<< " 字节，实际发送 " << bytes_written << " 字节" << std::endl;
return false;
}
return true;
}
bool RS485Driver::receiveData(std::vector<uint8_t> &data, size_t max_length, int timeout_ms)
{
if (!is_open_)
{
std::cerr << "串口未打开，无法接收数据" << std::endl;
return false;
}
data.clear();
uint8_t buffer[256];
ssize_t bytes_read;
auto start_time = std::chrono::steady_clock::now();
while (data.size() < max_length)
{
// 检查超时
auto current_time = std::chrono::steady_clock::now();
auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
current_time - start_time).count();
if (elapsed_ms >= timeout_ms)
{
break; // 超时
}
// 尝试读取数据
bytes_read = read(serial_port_, buffer, std::min(sizeof(buffer), max_length - data.size()));
if (bytes_read > 0)
{
data.insert(data.end(), buffer, buffer + bytes_read);
start_time = std::chrono::steady_clock::now(); // 重置超时计时器
}
else if (bytes_read < 0)
{
std::cerr << "接收数据错误" << std::endl;
return false;
}
else
{
// 没有数据，短暂休眠
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
return !data.empty();
}
uint8_t RS485Driver::calculateChecksum(const std::vector<uint8_t> &data)
{
uint8_t checksum = 0;
for (uint8_t byte : data)
{
checksum ^= byte;
}
return checksum;
}
bool RS485Driver::controlMotor(uint8_t motor_id, int32_t speed, bool stop)
{
if (!isOpen())
{
std::cerr << "RS485未初始化，无法控制电机" << std::endl;
return false;
}
if (motor_id < 1 || motor_id > max_motors_)
{
std::cerr << "无效的电机ID: " << static_cast<int>(motor_id)
<< ", 必须在1到" << static_cast<int>(max_motors_) << "之间" << std::endl;
return false;
}
// 限制速度范围
if (speed > 1000) speed = 1000;
if (speed < -1000) speed = -1000;
// 构建命令帧
std::vector<uint8_t> command;
command.push_back(START_BYTE);
if (stop)
{
command.push_back(STOP_CMD);
}
else
{
command.push_back(CONTROL_CMD);
}
command.push_back(motor_id);
// 将速度转换为4字节并添加到命令
uint8_t speed_bytes[4];
std::memcpy(speed_bytes, &speed, sizeof(speed));
for (int i = 0; i < 4; ++i)
{
command.push_back(speed_bytes[i]);
}
// 计算并添加校验和
uint8_t checksum = calculateChecksum(command);
command.push_back(checksum);
// 添加结束字节
command.push_back(STOP_BYTE);
// 发送命令
bool success = sendData(command);
if (success)
{
if (stop)
{
std::cout << "已发送停止命令到电机 " << static_cast<int>(motor_id) << std::endl;
}
else
{
std::cout << "已发送命令到电机 " << static_cast<int>(motor_id)
<< ", 速度: " << speed << std::endl;
}
}
else
{
std::cerr << "发送命令到电机 " << static_cast<int>(motor_id) << " 失败" << std::endl;
}
return success;
}
bool RS485Driver::controlAllMotors(const std::vector<int32_t> &speeds)
{
if (speeds.size() != max_motors_)
{
std::cerr << "控制所有电机需要提供 " << static_cast<int>(max_motors_)
<< " 个速度值，实际提供了 " << speeds.size() << " 个" << std::endl;
return false;
}
if (!isOpen())
{
std::cerr << "RS485未初始化，无法控制电机" << std::endl;
return false;
}
// 构建命令帧
bool success = false;
static uint8_t i = 1;
// 添加所有电机的速度
std::vector<uint8_t> command;
if (i < 5)
{
control_index_ = 1;
// 限制速度范围
int32_t clamped_speed = speeds[control_index_ - 1];
if (clamped_speed > 1000) clamped_speed = 1000;
if (clamped_speed < -1000) clamped_speed = -1000;
uint8_t high_byte = (clamped_speed >> 8) & 0xFF; // 高8位
uint8_t low_byte = clamped_speed & 0xFF; // 低8位
command.push_back(2);
command.push_back(CONTROL_CMD);
command.push_back(high_byte);
command.push_back(low_byte);
command.push_back(ZERO_CMD);
command.push_back(ZERO_CMD);
command.push_back(DEFAULT_TIME);
command.push_back(ZERO_CMD);
command.push_back(ZERO_CMD);
unsigned char crc8 = CRC8_Table(command.data(), 9);
command.push_back(crc8);
// 发送命令
success = sendData(command);
// std::cout << "已发送命令到电机 " << static_cast<int>(control_index_) << ", 速度: " << clamped_speed << std::endl;
}
else if ( i > 5)
{
control_index_ = 2;
// 限制速度范围
int32_t clamped_speed = speeds[control_index_ - 1];
if (clamped_speed > 1000) clamped_speed = 1000;
if (clamped_speed < -1000) clamped_speed = -1000;
uint8_t high_byte = (clamped_speed >> 8) & 0xFF; // 高8位
uint8_t low_byte = clamped_speed & 0xFF; // 低8位
command.push_back(3);
command.push_back(CONTROL_CMD);
command.push_back(high_byte);
command.push_back(low_byte);
command.push_back(ZERO_CMD);
command.push_back(ZERO_CMD);
command.push_back(DEFAULT_TIME);
command.push_back(ZERO_CMD);
command.push_back(ZERO_CMD);
unsigned char crc8 = CRC8_Table(command.data(), 9);
command.push_back(crc8);
// 发送命令
success = sendData(command);
// std::cout << "已发送命令到电机 " << static_cast<int>(control_index_) << ", 速度: " << clamped_speed << std::endl;
}
++i;
// std::cout << "i = " << static_cast<int>(i) << std::endl;
if (i > 8)
{
i = 1;
return success;
}
return false;
}
bool RS485Driver::stopAllMotors()
{
std::vector<int32_t> speeds(max_motors_, 0);
return controlAllMotors(speeds);
}
}

8
src/robot_control/src/wheel_control.cpp
View File

@@ -87,12 +87,6 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
tempHomePositions[0] - 40;
tempAdjustCount = 0;
}
// if (tempAdjustCount == 2)
// {
// tempAdjustCount = 0;
// }
}
for (size_t i = 0; i < joint_directions_.size(); i++)
@@ -199,7 +193,7 @@ bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, d
else
{
tempHomePositions[i] -= theta;
tempDesiredPositions[i] = tempHomePositions[i]; // * 0.98 * 1.02;
tempDesiredPositions[i] = tempHomePositions[i];
}
}