150 lines
4.4 KiB
C++
150 lines
4.4 KiB
C++
#include "diffdrive_arduino/diffdrive_arduino.h"
|
|
|
|
|
|
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
|
#include "hardware_interface/types/hardware_interface_return_values.hpp"
|
|
|
|
#include "rclcpp/rclcpp.hpp"
|
|
|
|
namespace diffdrive_arduino
|
|
{
|
|
|
|
|
|
DiffDriveArduino::DiffDriveArduino()
|
|
: logger_(rclcpp::get_logger("DiffDriveArduino"))
|
|
{}
|
|
|
|
CallbackReturn DiffDriveArduino::on_init(const hardware_interface::HardwareInfo & info)
|
|
{
|
|
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
|
|
{
|
|
return CallbackReturn::ERROR;
|
|
}
|
|
|
|
RCLCPP_INFO(logger_, "Configuring...");
|
|
|
|
time_ = std::chrono::system_clock::now();
|
|
|
|
cfg_.left_wheel_name = info_.hardware_parameters["left_wheel_name"];
|
|
cfg_.right_wheel_name = info_.hardware_parameters["right_wheel_name"];
|
|
cfg_.loop_rate = std::stof(info_.hardware_parameters["loop_rate"]);
|
|
cfg_.device = info_.hardware_parameters["device"];
|
|
cfg_.baud_rate = std::stoi(info_.hardware_parameters["baud_rate"]);
|
|
cfg_.timeout = std::stoi(info_.hardware_parameters["timeout"]);
|
|
cfg_.enc_counts_per_rev = std::stoi(info_.hardware_parameters["enc_counts_per_rev"]);
|
|
|
|
// Set up the wheels
|
|
l_wheel_.setup(cfg_.left_wheel_name, cfg_.enc_counts_per_rev);
|
|
r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev);
|
|
|
|
// Set up the Arduino
|
|
arduino_.setup(cfg_.device, cfg_.baud_rate, cfg_.timeout);
|
|
|
|
RCLCPP_INFO(logger_, "Finished Configuration");
|
|
|
|
return CallbackReturn::SUCCESS;
|
|
}
|
|
|
|
std::vector<hardware_interface::StateInterface> DiffDriveArduino::export_state_interfaces()
|
|
{
|
|
// We need to set up a position and a velocity interface for each wheel
|
|
|
|
std::vector<hardware_interface::StateInterface> state_interfaces;
|
|
|
|
state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.vel));
|
|
state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_POSITION, &l_wheel_.pos));
|
|
state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.vel));
|
|
state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_POSITION, &r_wheel_.pos));
|
|
|
|
return state_interfaces;
|
|
}
|
|
|
|
std::vector<hardware_interface::CommandInterface> DiffDriveArduino::export_command_interfaces()
|
|
{
|
|
// We need to set up a velocity command interface for each wheel
|
|
|
|
std::vector<hardware_interface::CommandInterface> command_interfaces;
|
|
|
|
command_interfaces.emplace_back(hardware_interface::CommandInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.cmd));
|
|
command_interfaces.emplace_back(hardware_interface::CommandInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.cmd));
|
|
|
|
return command_interfaces;
|
|
}
|
|
|
|
|
|
CallbackReturn DiffDriveArduino::on_activate(const rclcpp_lifecycle::State & /* previous_state */)
|
|
{
|
|
RCLCPP_INFO(logger_, "Starting Controller...");
|
|
arduino_.sendEmptyMsg();
|
|
// arduino.setPidValues(9,7,0,100);
|
|
// arduino.setPidValues(14,7,0,100);
|
|
arduino_.setPidValues(30, 20, 0, 100);
|
|
|
|
return CallbackReturn::SUCCESS;
|
|
}
|
|
|
|
CallbackReturn DiffDriveArduino::on_deactivate(const rclcpp_lifecycle::State & /* previous_state */)
|
|
{
|
|
RCLCPP_INFO(logger_, "Stopping Controller...");
|
|
|
|
return CallbackReturn::SUCCESS;
|
|
}
|
|
|
|
return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
|
|
{
|
|
|
|
// TODO fix chrono duration
|
|
|
|
// Calculate time delta
|
|
auto new_time = std::chrono::system_clock::now();
|
|
std::chrono::duration<double> diff = new_time - time_;
|
|
double deltaSeconds = diff.count();
|
|
time_ = new_time;
|
|
|
|
|
|
if (!arduino_.connected())
|
|
{
|
|
return return_type::ERROR;
|
|
}
|
|
|
|
arduino_.readEncoderValues(l_wheel_.vel , r_wheel_.vel);
|
|
|
|
// Force the wheel position
|
|
l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds;
|
|
r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds;
|
|
|
|
|
|
|
|
return return_type::OK;
|
|
|
|
|
|
}
|
|
|
|
return_type DiffDriveArduino::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
|
|
{
|
|
|
|
if (!arduino_.connected())
|
|
{
|
|
return return_type::ERROR;
|
|
}
|
|
|
|
arduino_.setMotorValues(l_wheel_.cmd, r_wheel_.cmd);
|
|
|
|
|
|
|
|
|
|
return return_type::OK;
|
|
|
|
|
|
|
|
}
|
|
|
|
} // namespace diffdrive_arduino
|
|
|
|
#include "pluginlib/class_list_macros.hpp"
|
|
|
|
PLUGINLIB_EXPORT_CLASS(
|
|
diffdrive_arduino::DiffDriveArduino,
|
|
hardware_interface::SystemInterface
|
|
)
|