Pimpest
85c8e13fbf
Changed the way we communicate with the stepper motor drivers. Fixes #3 Co-authored-by: Pimpest <82343504+Pimpest@users.noreply.github.com> Co-committed-by: Pimpest <82343504+Pimpest@users.noreply.github.com>
84 lines
3.2 KiB
C++
84 lines
3.2 KiB
C++
#include "mg_control/mg_control.hpp"
|
|
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
|
|
|
#include <cmath>
|
|
|
|
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()) {
|
|
serial_port_name = info_.hardware_parameters["device_path"];
|
|
} else {
|
|
serial_port_name = "/dev/ttyACM0";
|
|
}
|
|
|
|
left_wheel_pos_state = 0;
|
|
right_wheel_pos_state = 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()) {
|
|
odrive_serial_interface.Open(serial_port_name);
|
|
}
|
|
} catch (const LibSerial::OpenFailed& er) {
|
|
RCLCPP_FATAL(rclcpp::get_logger("MgStepperInterface"),
|
|
"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()) {
|
|
odrive_serial_interface.Close();
|
|
}
|
|
return CallbackReturn::SUCCESS;
|
|
}
|
|
|
|
std::vector<hardware_interface::StateInterface> mg::MgStepperInterface::export_state_interfaces() {
|
|
|
|
std::vector<hardware_interface::StateInterface> state_interfaces;
|
|
state_interfaces.reserve(2);
|
|
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> command_interfaces;
|
|
command_interfaces.reserve(2);
|
|
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::write(const rclcpp::Time&, const rclcpp::Duration&) {
|
|
union {
|
|
std::array<u_char, sizeof(double)> bytes;
|
|
double data;
|
|
} value{};
|
|
|
|
try {
|
|
odrive_serial_interface.Write("s;");
|
|
value.data = left_wheel_vel_cmd / (2 * M_PI);
|
|
for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }
|
|
value.data = -right_wheel_vel_cmd / (2 * M_PI);
|
|
for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }
|
|
|
|
} catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; }
|
|
return hardware_interface::return_type::OK;
|
|
}
|