diff --git a/README.md b/README.md index 19b7f11..482d5da 100644 --- a/README.md +++ b/README.md @@ -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 diff --git a/config/motor_driver.yaml b/config/motor_driver.yaml index 97eb85e..05470b1 100755 --- a/config/motor_driver.yaml +++ b/config/motor_driver.yaml @@ -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 - diff --git a/src/motor_driver.cpp b/src/motor_driver.cpp index f14eb1c..c246d4c 100755 --- a/src/motor_driver.cpp +++ b/src/motor_driver.cpp @@ -40,8 +40,6 @@ MotorDriver::MotorDriver() this->declare_parameter("publish_odom", true); this->declare_parameter("quad_pulses_per_meter", 0); this->declare_parameter("quad_pulses_per_revolution", 0); - this->declare_parameter("vmin", 1); - this->declare_parameter("vtime", 2); this->declare_parameter("wheel_radius", 0.0); this->declare_parameter("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()); diff --git a/src/motor_driver.h b/src/motor_driver.h index 21cb67d..1bf88bf 100755 --- a/src/motor_driver.h +++ b/src/motor_driver.h @@ -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_; diff --git a/src/roboclaw.cpp b/src/roboclaw.cpp index 8b0c85d..00583f6 100755 --- a/src/roboclaw.cpp +++ b/src/roboclaw.cpp @@ -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() { diff --git a/src/roboclaw.h b/src/roboclaw.h index 36120f4..000e73b 100755 --- a/src/roboclaw.h +++ b/src/roboclaw.h @@ -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);