Pimpest/ros2-control-testing
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Switched odrive interface with stepper motor interface

Browse Source
This commit is contained in:
Pimpest
2025-01-25 02:29:05 +01:00
parent a9e9b2c069
commit 75a47dd8c9
2 changed files with 12 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
mg_wheel_interface/assets/mg_base_controllers.yaml
View File

@ -16,7 +16,7 @@ diffdrive_controller:
odom_frame_id: odom
base_frame_id: base-link
# open_loop: true
open_loop: true
wheel_separation: 0.0964
wheel_radius: 0.035

51
mg_wheel_interface/src/mg_wheel_interface.cpp
View File

@ -43,24 +43,6 @@ CallbackReturn mg_wheel_interface::MgOdriveInterface::on_configure(const rclcpp_
CallbackReturn mg_wheel_interface::MgOdriveInterface::on_shutdown(const rclcpp_lifecycle::State &)
{
if(odrive_serial_interface.IsOpen()){
std::string cmd_left;
std::string cmd_right;
int hash;
cmd_left = "v 1 " + std::to_string(0) + " ";
cmd_right = "v 0 " + std::to_string(0) + " ";
hash = 0;
for(const auto &c: cmd_left) {
hash ^= c;
}
cmd_left += "*"+std::to_string(hash)+"\n";
hash = 0;
for(const auto &c: cmd_right) {
hash ^= c;
}
cmd_right += "*"+std::to_string(hash)+"\n";
odrive_serial_interface.Close();
}
return CallbackReturn::SUCCESS;
@ -96,13 +78,15 @@ std::vector<hardware_interface::CommandInterface> mg_wheel_interface::MgOdriveIn
hardware_interface::return_type mg_wheel_interface::MgOdriveInterface::read(const rclcpp::Time &time, const rclcpp::Duration &period)
{
std::string resp_l, resp_r;
odrive_serial_interface.Write("f 1\n");
odrive_serial_interface.ReadLine(resp_l,'\n',10);
odrive_serial_interface.Write("f 0\n");
odrive_serial_interface.ReadLine(resp_r,'\n',10);
left_wheel_pos_state = std::stod(resp_l)*2*M_PI;
right_wheel_pos_state = -std::stod(resp_r)*2*M_PI;
/*
* std::string resp_l, resp_r;
* odrive_serial_interface.Write("f 1\n");
* odrive_serial_interface.ReadLine(resp_l,'\n',10);
* odrive_serial_interface.Write("f 0\n");
* odrive_serial_interface.ReadLine(resp_r,'\n',10);
* left_wheel_pos_state = std::stod(resp_l)*2*M_PI;
* right_wheel_pos_state = -std::stod(resp_r)*2*M_PI;
*/
return hardware_interface::return_type::OK ;
}
@ -113,21 +97,8 @@ hardware_interface::return_type mg_wheel_interface::MgOdriveInterface::write(con
std::string cmd_right;
int hash;
cmd_left = "v 1 " + std::to_string(left_wheel_vel_cmd/2*M_PI) + " ";
cmd_right = "v 0 " + std::to_string(-right_wheel_vel_cmd/2*M_PI) + " ";
hash = 0;
for(const auto &c: cmd_left) {
hash ^= c;
}
cmd_left += "*"+std::to_string(hash)+"\n";
hash = 0;
for(const auto &c: cmd_right) {
hash ^= c;
}
cmd_right += "*"+std::to_string(hash)+"\n";
cmd_left = std::to_string(left_wheel_vel_cmd/2*M_PI) + " ";
cmd_right = std::to_string(-right_wheel_vel_cmd/2*M_PI);
try {
odrive_serial_interface.Write(cmd_left + cmd_right);