Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Refactored mg_control

Browse Source
This commit is contained in:
Pimpest
2025-02-25 10:58:28 +01:00
parent b5d474ff01
commit 581eca8720
2 changed files with 47 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
mg_control/src/mg_control.cpp
View File

@ -3,97 +3,77 @@
#include <cmath>
CallbackReturn mg::MgStepperInterface::on_init(const hardware_interface::HardwareInfo &info)
{
if(hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS){
CallbackReturn mg::MgStepperInterface::on_init(const hardware_interface::HardwareInfo& info) {
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
return CallbackReturn::ERROR;
}
if(info_.hardware_parameters.find("device_path") != info.hardware_parameters.end()) {
if (info_.hardware_parameters.find("device_path") != info.hardware_parameters.end()) {
serial_port_name = info_.hardware_parameters["device_path"];
} else {
serial_port_name = "/dev/ttyACM0";
}
left_wheel_pos_state = 0;
left_wheel_pos_state = 0;
right_wheel_pos_state = 0;
left_wheel_vel_cmd = 0;
right_wheel_vel_cmd = 0;
left_wheel_vel_cmd = 0;
right_wheel_vel_cmd = 0;
return CallbackReturn::SUCCESS;
}
CallbackReturn mg::MgStepperInterface::on_configure(const rclcpp_lifecycle::State &)
{
try{
if(!odrive_serial_interface.IsOpen()){
CallbackReturn mg::MgStepperInterface::on_configure(const rclcpp_lifecycle::State&) {
try {
if (!odrive_serial_interface.IsOpen()) {
odrive_serial_interface.Open(serial_port_name);
}
}
catch (const LibSerial::OpenFailed &er) {
} catch (const LibSerial::OpenFailed& er) {
RCLCPP_FATAL(rclcpp::get_logger("MgStepperInterface"),
"Failed to open serial port (Is the stepper driver plugged in)");
"Failed to open serial port (Is the stepper driver plugged in)");
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS;
}
CallbackReturn mg::MgStepperInterface::on_shutdown(const rclcpp_lifecycle::State &)
{
if(odrive_serial_interface.IsOpen()){
CallbackReturn mg::MgStepperInterface::on_shutdown(const rclcpp_lifecycle::State&) {
if (odrive_serial_interface.IsOpen()) {
odrive_serial_interface.Close();
}
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface> mg::MgStepperInterface::export_state_interfaces() {
std::vector<hardware_interface::StateInterface> mg::MgStepperInterface::export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.reserve(2);
state_interfaces.push_back(hardware_interface::StateInterface(
"left_motor", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state
));
state_interfaces.push_back(hardware_interface::StateInterface(
"right_motor", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state
));
return state_interfaces;
state_interfaces.emplace_back("left_motor", hardware_interface::HW_IF_POSITION, &left_wheel_pos_state);
state_interfaces.emplace_back("right_motor", hardware_interface::HW_IF_POSITION, &right_wheel_pos_state);
return state_interfaces;
}
std::vector<hardware_interface::CommandInterface> mg::MgStepperInterface::export_command_interfaces() {
std::vector<hardware_interface::CommandInterface> mg::MgStepperInterface::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.reserve(2);
command_interfaces.push_back(hardware_interface::CommandInterface(
"left_motor", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd
));
command_interfaces.push_back(hardware_interface::CommandInterface(
"right_motor", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd
));
return command_interfaces;
command_interfaces.emplace_back("left_motor", hardware_interface::HW_IF_VELOCITY, &left_wheel_vel_cmd);
command_interfaces.emplace_back("right_motor", hardware_interface::HW_IF_VELOCITY, &right_wheel_vel_cmd);
return command_interfaces;
}
hardware_interface::return_type mg::MgStepperInterface::read(const rclcpp::Time &, const rclcpp::Duration &)
{
return hardware_interface::return_type::OK ;
hardware_interface::return_type mg::MgStepperInterface::read(const rclcpp::Time&, const rclcpp::Duration&) {
return hardware_interface::return_type::OK;
}
hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time &, const rclcpp::Duration &)
{
hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time&, const rclcpp::Duration&) {
std::string cmd_left;
std::string cmd_right;
cmd_left = std::to_string(left_wheel_vel_cmd/2*M_PI) + " ";
cmd_right = std::to_string(-right_wheel_vel_cmd/2*M_PI);
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);
} catch(const std::runtime_error &e){
return hardware_interface::return_type::ERROR;
}
} catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; }
return hardware_interface::return_type::OK;
}