modelec/ros2_roboclaw_driver
1
0
Fork 0
mirror of https://github.com/modelec/ros2_roboclaw_driver.git synced 2026-01-18 16:47:26 +01:00
Code Issues Packages Projects Releases Wiki Activity

Removed unused vmin, vtime parameters.

Browse Source 
Altered some of the port options when opening the serial port.
This commit is contained in:
Michael Wimble
2025-03-06 11:32:31 -08:00
parent c3aea5a0e1
commit 20c404edf1
6 changed files with 55 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
README.md
View File

@@ -232,7 +232,7 @@ motor_driver_node:
# These values are used to scale cmd_vel commands
# and encoder values to motor commands and odometry, respectfully.
quad_pulses_per_meter: 1566
quad_pulses_per_revolution: 1000
quad_pulses_per_revolution: 979.62
# Based upon y our particular robot model.
# The wheel separation and radius, in meters.
@@ -241,10 +241,6 @@ motor_driver_node:
# Topic name to be used to publish the RoboClaw status messages.
roboclaw_status_topic: "roboclaw_status"
# See: http://unixwiz.net/techtips/termios-vmin-vtime.html
vmin: 1
vtime: 2
```
# Miscellaneous notes
The driver keeps track of the encoder values for the left and right

8
config/motor_driver.yaml
View File

@@ -9,7 +9,7 @@ motor_driver_node:
accel_quad_pulses_per_second: 1000
# The device name to be opened.
device_name: "/dev/roboclaw"
device_name: "/dev/ttyAMA0"
# The assigned port for the device (as configured on the RoboClaw).
device_port: 128
@@ -46,7 +46,7 @@ motor_driver_node:
# These values are used to scale cmd_vel commands
# and encoder values to motor commands and odometry, respectfully.
quad_pulses_per_meter: 1566
quad_pulses_per_revolution: 1000
quad_pulses_per_revolution: 979.62
# Based upon y our particular robot model.
# The wheel separation and radius, in meters.
@@ -56,7 +56,3 @@ motor_driver_node:
# Topic name to be used to publish the RoboClaw status messages.
roboclaw_status_topic: "roboclaw_status"
# See: http://unixwiz.net/techtips/termios-vmin-vtime.html
vmin: 1
vtime: 2

8
src/motor_driver.cpp
View File

@@ -40,8 +40,6 @@ MotorDriver::MotorDriver()
this->declare_parameter<bool>("publish_odom", true);
this->declare_parameter<int>("quad_pulses_per_meter", 0);
this->declare_parameter<float>("quad_pulses_per_revolution", 0);
this->declare_parameter<int>("vmin", 1);
this->declare_parameter<int>("vtime", 2);
this->declare_parameter<float>("wheel_radius", 0.0);
this->declare_parameter<float>("wheel_separation", 0.0);
}
@@ -104,8 +102,6 @@ void MotorDriver::onInit(rclcpp::Node::SharedPtr node) {
this->get_parameter("quad_pulses_per_meter", quad_pulses_per_meter_);
this->get_parameter("quad_pulses_per_revolution",
quad_pulses_per_revolution_);
this->get_parameter("vmin", vmin_);
this->get_parameter("vtime", vtime_);
this->get_parameter("wheel_radius", wheel_radius_);
this->get_parameter("wheel_separation", wheel_separation_);
@@ -132,8 +128,6 @@ void MotorDriver::onInit(rclcpp::Node::SharedPtr node) {
RCUTILS_LOG_INFO("quad_pulses_per_meter: %d", quad_pulses_per_meter_);
RCUTILS_LOG_INFO("quad_pulses_per_revolution: %3.4f",
quad_pulses_per_revolution_);
RCUTILS_LOG_INFO("vmin: %d", vmin_);
RCUTILS_LOG_INFO("vtime: %d", vtime_);
RCUTILS_LOG_INFO("wheel_radius: %f", wheel_radius_);
RCUTILS_LOG_INFO("wheel_separation: %f", wheel_separation_);
@@ -141,7 +135,7 @@ void MotorDriver::onInit(rclcpp::Node::SharedPtr node) {
RoboClaw::TPIDQ m2Pid = {m2_p_, m2_i_, m2_d_, m2_qpps_, m2_max_current_};
new RoboClaw(m1Pid, m2Pid, m1_max_current_, m2_max_current_,
device_name_.c_str(), device_port_, vmin_, vtime_);
device_name_.c_str(), device_port_);
RCUTILS_LOG_INFO("Main battery: %f",
RoboClaw::singleton()->getMainBatteryLevel());

2
src/motor_driver.h
View File

@@ -47,8 +47,6 @@ class MotorDriver : public rclcpp::Node {
std::thread publisher_thread_; // For publishing odom, joint state, etc.
uint32_t quad_pulses_per_meter_;
float quad_pulses_per_revolution_;
uint8_t vmin_;
uint8_t vtime_;
double wheel_radius_;
double wheel_separation_;

100
src/roboclaw.cpp
View File

@@ -22,15 +22,13 @@
RoboClaw::RoboClaw(const TPIDQ m1Pid, const TPIDQ m2Pid, float m1MaxCurrent,
float m2MaxCurrent, std::string device_name,
uint8_t device_port, uint8_t vmin, uint8_t vtime)
uint8_t device_port)
: device_port_(device_port),
maxCommandRetries_(3),
maxM1Current_(m1MaxCurrent),
maxM2Current_(m2MaxCurrent),
device_name_(device_name),
portAddress_(128),
vmin_(vmin),
vtime_(vtime) {
portAddress_(128) {
openPort();
RCUTILS_LOG_INFO("[RoboClaw::RoboClaw] RoboClaw software version: %s",
getVersion().c_str());
@@ -705,7 +703,7 @@ std::string RoboClaw::getVersion() {
void RoboClaw::openPort() {
RCUTILS_LOG_INFO("[RoboClaw::openPort] about to open port: %s",
device_name_.c_str());
device_port_ = open(device_name_.c_str(), O_RDWR | O_NOCTTY | O_SYNC);
device_port_ = open(device_name_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
if (device_port_ < 0) {
RCUTILS_LOG_ERROR(
"[RoboClaw::openPort] Unable to open USB port: %s, errno: (%d) "
@@ -725,49 +723,9 @@ void RoboClaw::openPort() {
"[RoboClaw::openPort] Unable to get terminal options "
"(tcgetattr), error: %d: %s",
errno, strerror(errno));
// throw new TRoboClawException("[RoboClaw::openPort] Unable to get
// terminal options (tcgetattr)");
throw new TRoboClawException("[RoboClaw::openPort] Unable to get terminal options (tcgetattr)");
}
// c_cflag contains a few important things- CLOCAL and CREAD, to prevent
// this program from "owning" the port and to enable receipt of data.
// Also, it holds the settings for number of data bits, parity, stop bits,
// and hardware flow control.
portOptions.c_cflag |= CLOCAL; // Prevent changing ownership.
portOptions.c_cflag |= CREAD; // Enable reciever.
portOptions.c_cflag &= ~CRTSCTS; // Disable hardware CTS/RTS flow control.
portOptions.c_cflag |= CS8; // Enable 8 bit characters.
portOptions.c_cflag &= ~CSIZE; // Remove size flag.
portOptions.c_cflag &= ~CSTOPB; // Disable 2 stop bits.
portOptions.c_cflag |=
HUPCL; // Enable lower control lines on close - hang up.
portOptions.c_cflag &= ~PARENB; // Disable parity.
// portOptions.c_iflag |= BRKINT;
portOptions.c_iflag &= ~IGNBRK; // Disable ignoring break.
portOptions.c_iflag &= ~IGNCR; // Disable ignoring CR;
portOptions.c_iflag &= ~(IGNPAR | PARMRK); // Disable parity checks.
// portOptions.c_iflag |= IGNPAR;
portOptions.c_iflag &= ~(INLCR | ICRNL); // Disable translating NL <-> CR.
portOptions.c_iflag &= ~INPCK; // Disable parity checking.
portOptions.c_iflag &= ~ISTRIP; // Disable stripping 8th bit.
portOptions.c_iflag &= ~(IXON | IXOFF); // disable XON/XOFF flow control
portOptions.c_lflag &= ~ECHO; // Disable echoing characters.
portOptions.c_lflag &= ~ECHONL; // ??
portOptions.c_lflag &= ~ICANON; // Disable canonical mode - line by line.
portOptions.c_lflag &= ~IEXTEN; // Disable input processing
portOptions.c_lflag &= ~ISIG; // Disable generating signals.
portOptions.c_lflag &= ~NOFLSH; // Disable flushing on SIGINT.
portOptions.c_oflag &= ~OFILL; // Disable fill characters.
portOptions.c_oflag &= ~(ONLCR | OCRNL); // Disable translating NL <-> CR.
portOptions.c_oflag &= ~OPOST; // Disable output processing.
portOptions.c_cc[VKILL] = 8;
portOptions.c_cc[VMIN] = vmin_;
portOptions.c_cc[VTIME] = vtime_;
if (cfsetispeed(&portOptions, B38400) < 0) {
RCUTILS_LOG_ERROR(
"[RoboClaw::openPort] Unable to set terminal speed "
@@ -786,16 +744,58 @@ void RoboClaw::openPort() {
"(cfsetospeed)");
}
// Now that we've populated our options structure, let's push it back to the
// system.
if (tcsetattr(device_port_, TCSANOW, &portOptions) < 0) {
// Configure other settings
portOptions.c_cflag &= ~PARENB; // Disable parity.
portOptions.c_cflag &= ~CSTOPB; // 1 stop bit
portOptions.c_cflag &= ~CSIZE; // Clear data size bits
portOptions.c_cflag |= CS8; // 8 data bits
portOptions.c_cflag &= ~CRTSCTS; // Disable hardware flow control
portOptions.c_cflag |= CREAD | CLOCAL; // Enable read and ignore control lines
portOptions.c_lflag &= ~ICANON; // Disable canonical mode
portOptions.c_lflag &= ~ECHO; // Disable echo
portOptions.c_lflag &= ~ECHOE; // Disable erasure
portOptions.c_lflag &= ~ECHONL; // Disable new-line echo
portOptions.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP
portOptions.c_iflag &= ~(IXON | IXOFF | IXANY); // Disable software flow control
portOptions.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL); // Disable special handling of received bytes
portOptions.c_oflag &= ~OPOST; // Disable output processing
portOptions.c_cc[VMIN] = 0; // Non-blocking read
portOptions.c_cc[VTIME] = 5; // Timeout of 0.5 seconds
if (tcsetattr( device_port_ , TCSANOW, &portOptions) != 0) {
RCUTILS_LOG_ERROR(
"[RoboClaw::openPort] Unable to set terminal options "
"(tcsetattr)");
throw new TRoboClawException(
"[RoboClaw::openPort] Unable to set terminal options "
"(tcsetattr)");
}
}
// // c_cflag contains a few important things- CLOCAL and CREAD, to prevent
// // this program from "owning" the port and to enable receipt of data.
// // Also, it holds the settings for number of data bits, parity, stop bits,
// // and hardware flow control.
// portOptions.c_cflag |=
// HUPCL; // Enable lower control lines on close - hang up.
// portOptions.c_iflag &= ~(IGNPAR); // Disable parity checks.
// // portOptions.c_iflag |= IGNPAR;
// portOptions.c_iflag &= ~INPCK; // Disable parity checking.
// portOptions.c_lflag &= ~IEXTEN; // Disable input processing
// portOptions.c_lflag &= ~NOFLSH; // Disable flushing on SIGINT.
// portOptions.c_oflag &= ~OFILL; // Disable fill characters.
// portOptions.c_oflag &= ~(ONLCR | OCRNL); // Disable translating NL <-> CR.
// portOptions.c_cc[VKILL] = 8;
}
uint8_t RoboClaw::readByteWithTimeout() {

5
src/roboclaw.h
View File

@@ -74,8 +74,7 @@ class RoboClaw {
// Constructor.
RoboClaw(const TPIDQ m1Pid, const TPIDQ m2Pid, float m1MaxCurrent,
float m2MaxCurrent, std::string device_name, uint8_t device_port,
uint8_t vmin, uint8_t vtime);
float m2MaxCurrent, std::string device_name, uint8_t device_port);
~RoboClaw();
@@ -282,8 +281,6 @@ class RoboClaw {
int motorAlarms_; // Motors alarms. Bit-wise OR of contributors.
std::string device_name_; // Device name of RoboClaw device.
int portAddress_; // Port number of RoboClaw device under control
int vmin_; // Terminal control value.
int vtime_; // Terminal control value.
// Get velocity (speed) result from the RoboClaw controller.
int32_t getVelocityResult(uint8_t command);