diff --git a/include/roboclaw.h b/include/roboclaw.h index 7574304..250ca46 100755 --- a/include/roboclaw.h +++ b/include/roboclaw.h @@ -1,10 +1,13 @@ #pragma once +#include #include #include #include +#include #include + #include #include "ros2_roboclaw_driver/srv/reset_encoders.hpp" @@ -26,8 +29,6 @@ #define SetDWORDval(arg) \ (uint8_t)(arg >> 24), (uint8_t)(arg >> 16), (uint8_t)(arg >> 8), (uint8_t)arg -#define kDebugIO true - class RoboClaw { public: // Bit positions used to build alarms. @@ -86,10 +87,17 @@ public: ~RoboClaw(); - void appendToLog(const char *format, ...) { + void appendToReadLog(const char *format, ...) { va_list args; va_start(args, format); - debug_log_.appendToLog(format, args); + debug_log_.appendToReadLog(format, args); + va_end(args); + } + + void appendToWriteLog(const char *format, ...) { + va_list args; + va_start(args, format); + debug_log_.appendToWriteLog(format, args); va_end(args); } @@ -158,6 +166,12 @@ protected: void writeN2(bool sendCRC, uint8_t cnt, ...); private: + // True => print debug messages. + bool doDebug_; + + // True => print byte values as they are read and written. + bool doLowLevelDebug_; + EncodeResult cache_getEncoderCommandResult(WHICH_ENC command); // Get RoboClaw error status bits. @@ -318,6 +332,9 @@ private: // Read one byte from device with timeout. uint8_t readByteWithTimeout(); + // Read one byte from device with timeout. + uint8_t readByteWithTimeout2(); + // Perform error recovery to re-open a failed device port. void restartPort(); @@ -390,25 +407,40 @@ private: class DebugLog { public: - DebugLog() : next_log_index_(0) {} + DebugLog() : next_read_log_index_(0), next_write_log_index_(0) {} ~DebugLog() {} - void appendToLog(const char *format, va_list args) { - int written = vsnprintf(&log_[next_log_index_], - sizeof(log_) - next_log_index_, format, args); + void appendToReadLog(const char *format, va_list args) { + int written = + vsnprintf(&read_log_[next_read_log_index_], + sizeof(read_log_) - next_read_log_index_, format, args); if (written > 0) { - next_log_index_ += written; + next_read_log_index_ += written; + } + } + + void appendToWriteLog(const char *format, va_list args) { + int written = + vsnprintf(&write_log_[next_write_log_index_], + sizeof(write_log_) - next_write_log_index_, format, args); + if (written > 0) { + next_write_log_index_ += written; } } void showLog() { - RCUTILS_LOG_INFO("%s", log_); - next_log_index_ = 0; + RCUTILS_LOG_INFO("%s, READ: %s", write_log_, read_log_); + read_log_[0] = '\0'; + next_read_log_index_ = 0; + write_log_[0] = '\0'; + next_write_log_index_ = 0; } // private: - char log_[256]; - uint16_t next_log_index_; + char read_log_[256]; + char write_log_[256]; + uint16_t next_read_log_index_; + uint16_t next_write_log_index_; }; class CmdSetPid : public Cmd { @@ -418,9 +450,10 @@ private: : Cmd(roboclaw, "SetPid", motor), p_(p), i_(i), d_(d), qpps_(qpps) {} void send() override { - roboclaw_.appendToLog("SetPid: motor: %d (%s) p: %f, i: %f, d: %f, " - "qpps: %d, bytes: ", - motor_, motorNames_[motor_], p_, i_, d_, qpps_); + roboclaw_.appendToWriteLog("SetPid: motor: %d (%s) p: %f, i: %f, d: %f, " + "qpps: %d, WROTE: ", + motor_, motorNames_[motor_], p_, i_, d_, + qpps_); uint32_t kp = int(p_ * 65536.0); uint32_t ki = int(i_ * 65536.0); uint32_t kd = int(d_ * 65536.0); @@ -436,13 +469,96 @@ private: uint32_t qpps_; }; + class CmdReadFirmwareVersion : public Cmd { + public: + CmdReadFirmwareVersion(RoboClaw &roboclaw, std::string &version) + : Cmd(roboclaw, "ReadFirmwareVersion", kNone), version_(version) {} + + void send() override { + roboclaw_.appendToWriteLog("ReadFirmwareVersion: WROTE: "); + roboclaw_.writeN2(false, 2, roboclaw_.portAddress_, kGETVERSION); + + try { + uint16_t crc = 0; + uint8_t i; + uint8_t datum; + std::stringstream version; + + roboclaw_.updateCrc(crc, roboclaw_.portAddress_); + roboclaw_.updateCrc(crc, kGETVERSION); + for (i = 0; i < 48; i++) { + datum = roboclaw_.readByteWithTimeout2(); + roboclaw_.updateCrc(crc, datum); + if (datum == 0) { + uint16_t responseCrc = 0; + datum = roboclaw_.readByteWithTimeout2(); + responseCrc = datum << 8; + datum = roboclaw_.readByteWithTimeout2(); + responseCrc |= datum; + if (responseCrc == crc) { + version_ = version.str(); + // roboclaw_.debug_log_.appendToReadLog("RR%d", 0); + // roboclaw_.debug_log_.appendToWriteLog("WW%d", 0); + roboclaw_.debug_log_.showLog(); + return; + } else { + roboclaw_.debug_log_.showLog(); + RCUTILS_LOG_ERROR("[RoboClaw::CmdReadFirmwareVersion] invalid " + "CRC expected: 0x%02X, " + "got: 0x%02X", + crc, responseCrc); + } + } else { + version << (char)datum; + } + } + + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadFirmwareVersion] unexpected long string"); + throw new TRoboClawException( + "[RoboClaw::getVersion] unexpected long string"); + } catch (...) { + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadFirmwareVersion] Uncaught exception !!!"); + } + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadFirmwareVersion] RETRY COUNT EXCEEDED"); + } + + std::string &version_; + }; + + class CmdSetEncoderValue : public Cmd { + public: + CmdSetEncoderValue(RoboClaw &roboclaw, kMotor motor, long value) + : Cmd(roboclaw, "SetEncoderValue", motor), value_(value) {} + + void send() override { + roboclaw_.appendToWriteLog( + "SetEncoderValue: motor: %d (%s) value: %ld, WROTE: ", motor_, + motorNames_[motor_], value_); + // roboclaw_.SetEncoder(motor_ == kM1 ? kSETM1ENCODER : kSETM2ENCODER, + // value_); + try { + roboclaw_.writeN2(true, 6, roboclaw_.portAddress_, + motor_ == kM1 ? kSETM1ENCODER : kSETM2ENCODER, + SetDWORDval(value_)); + roboclaw_.debug_log_.showLog(); + return; + } catch (...) { + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdSetEncoderValue] Uncaught exception !!!"); + } + } + + long value_; + }; + friend class Cmd; // Make Cmd a friend class of RoboClaw protected: DebugLog debug_log_; -#ifdef kDebugIO static const char *commandNames_[]; static const char *motorNames_[]; -#endif }; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..ba52237 --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1,101 @@ +cmake_minimum_required(VERSION 3.5) +project(ros2_roboclaw_driver) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) # Ensure the standard is enforced + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +add_compile_options(-g) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs) +find_package(rclcpp REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) + +include_directories( + ${geometry_msgs_INCLUDE_DIRS} + ${nav_msgs_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} + ${sensor_msgs_INCLUDE_DIRS} + ${std_msgs_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR}/include + /usr/include # Ensure standard system headers are included +) + +set(msg_files + "msg/RoboClawStatus.msg" +) + +set(srv_files + "srv/ResetEncoders.srv" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} +) + +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + +add_executable( + ros2_roboclaw_driver_node + src/motor_driver_node.cpp + src/motor_driver.cpp + src/roboclaw.cpp +) + +ament_target_dependencies( + ros2_roboclaw_driver_node + rclcpp + geometry_msgs + nav_msgs + sensor_msgs + std_msgs +) + +target_link_libraries(ros2_roboclaw_driver_node "${cpp_typesupport_target}") + +# Install config files. +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}/ +) + +# Install launch files. +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +install(TARGETS + ros2_roboclaw_driver_node + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + # set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + # set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/src/roboclaw.cpp b/src/roboclaw.cpp index 088a94e..56368f1 100755 --- a/src/roboclaw.cpp +++ b/src/roboclaw.cpp @@ -7,7 +7,6 @@ #include #include #include -#include #include #include @@ -17,283 +16,285 @@ #include "ros2_roboclaw_driver/srv/reset_encoders.h" -#if kDebugIO const char *RoboClaw::motorNames_[] = {"M1", "M2", "NONE"}; -const char *RoboClaw::commandNames_[] = { - "Drive Forward M1", // 0 - "Drive Backward M1", // 1 - "Set Min MB", // 2 - "Set Max MB", // 3 - "Drive Forward M2", // 4 - "Drive Backward M2", // 5 - "Drive 7 Bit M1", // 6 - "Drive 7 Bit M2", // 7 - "Drive Forward Mixed", // 8 - "Drive Backward Mixed", // 9 - "Turn Right Mixed", // 10 - "Turn Left Mixed", // 11 - "Drive FB Mixed", // 12 - "Drive LR Mixed", // 13 - "Set Serial Timeout", // 14 - "Read Serial Timeout", // 15 - "16", - "17", - "18", - "19", - "20", - "Read Firmware Version", // 21 - "22", - "23", - "Read Main Battery Voltage", // 24 - "Read Logic Battery Voltage", // 25 - "Set Min LB", // 26 - "Set Max LB", // 27 - "28", - "29", - "30", - "31", - "32", - "33", - "34", - "35", - "36", - "37", - "38", - "39", - "40", - "41", - "42", - "43", - "44", - "45", - "46", - "47", - "Read Motor PWMs", // 48 - "Read Motor Currents", // 49 - "50", - "51", - "52", - "53", - "54", - "55", - "56", - "Set Main Battery Volages", // 57 - "Set Logic Battery Voltages", // 58 - "Read Main Battery Voltages", // 59 - "Read Logic Battery Voltages", // 60 - "61", - "62", - "63", - "64", - "65", - "66", - "67", - "Set Default Duty Cycle Accel M1", // 68 - "Set Default Duty Cycle Accel M2", // 69 - "Set Default Speed M1", // 70 - "Set Default Speed M2", // 71 - "Read Default Speeds", // 72 - "73", - "Set S3, S4, S5 Modes", // 74 - "Read S3, S4, S5 Modes", // 75 - "Set Deadband", // 76 - "Read Deadband", // 77 - "78", - "79", - "Restore Defaults", // 80 - "Read Default Duty Cycle Accels", // 81 - "Read Temperature", // 82 - "Read Temperature 2", // 83 - "84", - "85", - "86", - "87", - "88", - "89", - "Read Status", // 90 - "Read Encoder Modes", // 91 - "Set M1 Encoder Mode", // 92 - "Set M2 Encoder Mode", // 93 - "Write Settings to EEPROM", // 94 - "Read Settings from EEPROM", // 95 - "96", - "97", - "Set Standard Config", // 98 - "Read Standard Config", // 99 - "Set CTRL Modes", // 100 - "Read CTRL Modes", // 101 - "Set CTRL1", // 102 - "Set CTRL2", // 103 - "Read CTRLs", // 104 - "Set Auto Home Duty/Speet/Timeout M1", // 105 - "Set Auto Home Duty/Speet/Timeout M2", // 106 - "Read Auto Home", // 107 - "108", - "Set Speed Error Limits", // 109 - "Read Speed Error Limits", // 110 - "111", - "Set Speed Position Error Limits", // 112 - "Read Speed Position Error Limits", // 113 - "114", - "Set Battery Voltage Offsets", // 115 - "Read Battery Voltage Offsets", // 116 - "Set Current Blanking %%s", // 117 - "Read Current Blanking %%s", // 118 - "119", - "120", - "121", - "122", - "123", - "124", - "125", - "126", - "127", - "128", - "129", - "130", - "131", - "132", - "Set M1 Max Current", // 133 - "Set M2 Max Current", // 134 - "Read M1 Max Current", // 135 - "Read M2 Max Current", // 136 - "137", - "138", - "139", - "140", - "141", - "142", - "143", - "144", - "145", - "146", - "147", - "Set PWM Mode", // 148 - "Read PWM Mode", // 149 - "150", - "151", - "152", - "153", - "154", - "155", - "156", - "157", - "158", - "159", - "160", - "161", - "162", - "163", - "164", - "165", - "166", - "167", - "168", - "169", - "170", - "171", - "172", - "173", - "174", - "175", - "176", - "177", - "178", - "179", - "180", - "181", - "182", - "183", - "184", - "185", - "186", - "187", - "188", - "189", - "190", - "191", - "192", - "193", - "194", - "195", - "196", - "197", - "198", - "199", - "200", - "201", - "202", - "203", - "204", - "205", - "206", - "207", - "208", - "209", - "210", - "211", - "212", - "213", - "214", - "215", - "216", - "217", - "218", - "219", - "220", - "221", - "222", - "223", - "224", - "225", - "226", - "227", - "228", - "229", - "230", - "231", - "232", - "233", - "234", - "235", - "236", - "237", - "238", - "239", - "240", - "241", - "242", - "243", - "244", - "245", - "246", - "247", - "248", - "249", - "250", - "251", - "Read User EEPROM Memory Location", // 252 - "Write User EEPROM Memory Location", // 253 - "254", - "255"}; -#endif +const char *RoboClaw::commandNames_[] = {"0", + "1", + "2", + "3", + "4", + "5", + "6", + "7", + "8", + "9", + "10", + "11", + "12", + "13", + "14", + "15", + "16", + "17", + "18", + "19", + "20", + "Read Firmware Version", + "22", + "23", + "24", + "25", + "26", + "27", + "Set Velocity PID M1", + "Set Velocity PID M2", + "30", + "31", + "32", + "33", + "34", + "35", + "36", + "37", + "38", + "39", + "40", + "41", + "42", + "43", + "44", + "45", + "46", + "47", + "48", + "49", + "50", + "51", + "52", + "53", + "54", + "55", + "56", + "57", + "58", + "59", + "60", + "61", + "62", + "63", + "64", + "65", + "66", + "67", + "68", + "69", + "70", + "71", + "72", + "73", + "74", + "75", + "76", + "77", + "78", + "79", + "80", + "81", + "82", + "83", + "84", + "85", + "86", + "87", + "88", + "89", + "90", + "91", + "92", + "93", + "94", + "95", + "96", + "97", + "98", + "99", + "100", + "101", + "102", + "103", + "104", + "105", + "106", + "107", + "108", + "109", + "110", + "111", + "112", + "113", + "114", + "115", + "116", + "117", + "118", + "119", + "120", + "121", + "122", + "123", + "124", + "125", + "126", + "127", + "128", + "129", + "130", + "131", + "132", + "133", + "134", + "135", + "136", + "137", + "138", + "139", + "140", + "141", + "142", + "143", + "144", + "145", + "146", + "147", + "148", + "149", + "150", + "151", + "152", + "153", + "154", + "155", + "156", + "157", + "158", + "159", + "160", + "161", + "162", + "163", + "164", + "165", + "166", + "167", + "168", + "169", + "170", + "171", + "172", + "173", + "174", + "175", + "176", + "177", + "178", + "179", + "180", + "181", + "182", + "183", + "184", + "185", + "186", + "187", + "188", + "189", + "190", + "191", + "192", + "193", + "194", + "195", + "196", + "197", + "198", + "199", + "200", + "201", + "202", + "203", + "204", + "205", + "206", + "207", + "208", + "209", + "210", + "211", + "212", + "213", + "214", + "215", + "216", + "217", + "218", + "219", + "220", + "221", + "222", + "223", + "224", + "225", + "226", + "227", + "228", + "229", + "230", + "231", + "232", + "233", + "234", + "235", + "236", + "237", + "238", + "239", + "240", + "241", + "242", + "243", + "244", + "245", + "246", + "247", + "248", + "249", + "250", + "251", + "252", + "253", + "254", + "255"}; RoboClaw::RoboClaw(const TPIDQ m1Pid, const TPIDQ m2Pid, float m1MaxCurrent, float m2MaxCurrent, std::string device_name, uint8_t device_port) - : device_port_(device_port), maxCommandRetries_(3), - maxM1Current_(m1MaxCurrent), maxM2Current_(m2MaxCurrent), - device_name_(device_name), portAddress_(128) { + : doDebug_(true), doLowLevelDebug_(true), device_port_(device_port), + maxCommandRetries_(3), maxM1Current_(m1MaxCurrent), + maxM2Current_(m2MaxCurrent), device_name_(device_name), + portAddress_(128) { openPort(); RCUTILS_LOG_INFO("[RoboClaw::RoboClaw] RoboClaw software version: %s", getVersion().c_str()); setM1PID(m1Pid.p, m1Pid.i, m1Pid.d, m1Pid.qpps); setM2PID(m2Pid.p, m2Pid.i, m2Pid.d, m2Pid.qpps); - ros2_roboclaw_driver::srv::ResetEncoders::Request resetRequest; - resetRequest.left = 0; - resetRequest.right = 0; - ros2_roboclaw_driver::srv::ResetEncoders::Response response; - resetEncoders(resetRequest, response); + CmdSetEncoderValue m1(*this, kM1, 0); + m1.execute(); + CmdSetEncoderValue m2(*this, kM2, 0); + m2.execute(); + // ros2_roboclaw_driver::srv::ResetEncoders::Request resetRequest; + // resetRequest.left = 0; + // resetRequest.right = 0; + // ros2_roboclaw_driver::srv::ResetEncoders::Response response; + // resetEncoders(resetRequest, response); g_singleton = this; readSensorGroup(); } @@ -917,55 +918,10 @@ int32_t RoboClaw::cache_getM2Encoder() { } std::string RoboClaw::getVersion() { - int retry; - - for (retry = 0; retry < maxCommandRetries_; retry++) { - try { - uint16_t crc = 0; - updateCrc(crc, portAddress_); - updateCrc(crc, kGETVERSION); - writeN(false, 2, portAddress_, kGETVERSION); - - uint8_t i; - uint8_t datum; - std::stringstream version; - - for (i = 0; i < 48; i++) { - datum = readByteWithTimeout(); - updateCrc(crc, datum); - if (datum == 0) { - uint16_t responseCrc = 0; - datum = readByteWithTimeout(); - responseCrc = datum << 8; - datum = readByteWithTimeout(); - responseCrc |= datum; - if (responseCrc == crc) { - return version.str(); - } else { - RCUTILS_LOG_ERROR( - "[RoboClaw::getVersion] invalid CRC expected: 0x%02X, " - "got: 0x%02X", - crc, responseCrc); - } - } else { - version << (char)datum; - } - } - - RCUTILS_LOG_ERROR("[RoboClaw::getVersion] unexpected long string"); - throw new TRoboClawException( - "[RoboClaw::getVersion] unexpected long string"); - } catch (TRoboClawException *e) { - RCUTILS_LOG_ERROR( - "[RoboClaw::getVersion] Exception: %s, retry number: %d", e->what(), - retry); - } catch (...) { - RCUTILS_LOG_ERROR("[RoboClaw::getVersion] Uncaught exception !!!"); - } - } - - RCUTILS_LOG_ERROR("[RoboClaw::getVersion] RETRY COUNT EXCEEDED"); - throw new TRoboClawException("[RoboClaw::getVersion] RETRY COUNT EXCEEDED"); + std::string version; + CmdReadFirmwareVersion command(*this, version); + command.execute(); + return version; } void RoboClaw::openPort() { @@ -1100,11 +1056,70 @@ uint8_t RoboClaw::readByteWithTimeout() { static const bool kDEBUG_READBYTE = true; if (kDEBUG_READBYTE) { if ((buffer[0] < 0x21) || (buffer[0] > 0x7F)) { - RCUTILS_LOG_INFO("read ..> char: ?? 0x%02X <--", buffer[0]); + // RCUTILS_LOG_INFO("read ..> char: ?? 0x%02X <--", buffer[0]); // fprintf(stderr, "..> char: ?? 0x%02X <--", buffer[0]); } else { - RCUTILS_LOG_INFO("read ..> char: %c 0x%02X <--", buffer[0], buffer[0]); - // fprintf(stderr, "..> char: %c 0x%02X <--", buffer[0], buffer[0]); + // RCUTILS_LOG_INFO("read ..> char: %c 0x%02X <--", buffer[0], + // buffer[0]); fprintf(stderr, "..> char: %c 0x%02X <--", buffer[0], + // buffer[0]); + } + } + + return buffer[0]; + } else { + RCUTILS_LOG_ERROR("[RoboClaw::readByteWithTimeout] Unhandled case"); + throw new TRoboClawException( + "[RoboClaw::readByteWithTimeout] Unhandled case"); + } + + return 0; +} + +uint8_t RoboClaw::readByteWithTimeout2() { + struct pollfd ufd[1]; + ufd[0].fd = device_port_; + ufd[0].events = POLLIN; + + int retval = poll(ufd, 1, 11); + if (retval < 0) { + RCUTILS_LOG_ERROR("[RoboClaw::readByteWithTimeout] Poll failed (%d) %s", + errno, strerror(errno)); + throw new TRoboClawException("[RoboClaw::readByteWithTimeout] Read error"); + } else if (retval == 0) { + std::stringstream ev; + ev << "[RoboClaw::readByteWithTimeout] TIMEOUT revents: " << std::hex + << ufd[0].revents; + RCUTILS_LOG_ERROR(ev.str().c_str()); + throw new TRoboClawException("[RoboClaw::readByteWithTimeout] TIMEOUT"); + } else if (ufd[0].revents & POLLERR) { + RCUTILS_LOG_ERROR("[RoboClaw::readByteWithTimeout] Error on socket"); + restartPort(); + throw new TRoboClawException( + "[RoboClaw::readByteWithTimeout] Error on socket"); + } else if (ufd[0].revents & POLLIN) { + unsigned char buffer[1]; + ssize_t bytesRead = ::read(device_port_, buffer, sizeof(buffer)); + if (bytesRead != 1) { + RCUTILS_LOG_ERROR( + "[RoboClaw::readByteWithTimeout] Failed to read 1 byte, read: " + "%d", + (int)bytesRead); + throw TRoboClawException( + "[RoboClaw::readByteWithTimeout] Failed to read 1 byte"); + } + + static const bool kDEBUG_READBYTE = true; + if (doLowLevelDebug_) { + appendToReadLog("%02X ", buffer[0]); + } + if (kDEBUG_READBYTE) { + if ((buffer[0] < 0x21) || (buffer[0] > 0x7F)) { + // RCUTILS_LOG_INFO("read ..> char: ?? 0x%02X <--", buffer[0]); + // fprintf(stderr, "..> char: ?? 0x%02X <--", buffer[0]); + } else { + // RCUTILS_LOG_INFO("read ..> char: %c 0x%02X <--", buffer[0], + // buffer[0]); fprintf(stderr, "..> char: %c 0x%02X <--", buffer[0], + // buffer[0]); } } @@ -1154,8 +1169,12 @@ bool RoboClaw::resetEncoders( ros2_roboclaw_driver::srv::ResetEncoders::Request &request, ros2_roboclaw_driver::srv::ResetEncoders::Response &response) { try { - SetEncoder(kSETM1ENCODER, request.left); - SetEncoder(kSETM2ENCODER, request.right); + CmdSetEncoderValue m1(*this, kM1, request.left); + CmdSetEncoderValue m2(*this, kM2, request.right); + m1.execute(); + m2.execute(); + // SetEncoder(kSETM1ENCODER, request.left); + // SetEncoder(kSETM2ENCODER, request.right); response.ok = true; } catch (...) { RCUTILS_LOG_ERROR("[RoboClaw::resetEncoders] uncaught exception"); @@ -1198,51 +1217,11 @@ void RoboClaw::SetEncoder(ROBOCLAW_COMMAND command, long value) { void RoboClaw::setM1PID(float p, float i, float d, uint32_t qpps) { CmdSetPid command(*this, kM1, p, i, d, qpps); command.execute(); - - // int retry; - - // for (retry = 0; retry < maxCommandRetries_; retry++) { - // try { - // uint32_t kp = int(p * 65536.0); - // uint32_t ki = int(i * 65536.0); - // uint32_t kd = int(d * 65536.0); - // writeN(true, 18, portAddress_, kSETM1PID, SetDWORDval(kd), - // SetDWORDval(kp), SetDWORDval(ki), SetDWORDval(qpps)); - // return; - // } catch (TRoboClawException *e) { - // RCUTILS_LOG_ERROR("[RoboClaw::setM1PID] Exception: %s, retry number: - // %d", - // e->what(), retry); - // } catch (...) { - // RCUTILS_LOG_ERROR("[RoboClaw::setM1PID] Uncaught exception !!!"); - // } - // } - - // RCUTILS_LOG_ERROR("[RoboClaw::setM1PID] RETRY COUNT EXCEEDED"); - // throw new TRoboClawException("[RoboClaw::setM1PID] RETRY COUNT EXCEEDED"); } void RoboClaw::setM2PID(float p, float i, float d, uint32_t qpps) { - int retry; - - for (retry = 0; retry < maxCommandRetries_; retry++) { - try { - uint32_t kp = int(p * 65536.0); - uint32_t ki = int(i * 65536.0); - uint32_t kd = int(d * 65536.0); - writeN(true, 18, portAddress_, kSETM2PID, SetDWORDval(kd), - SetDWORDval(kp), SetDWORDval(ki), SetDWORDval(qpps)); - return; - } catch (TRoboClawException *e) { - RCUTILS_LOG_ERROR("[RoboClaw::setM2PID] Exception: %s, retry number: %d", - e->what(), retry); - } catch (...) { - RCUTILS_LOG_ERROR("[RoboClaw::setM2PID] Uncaught exception !!!"); - } - } - - RCUTILS_LOG_ERROR("[RoboClaw::setM2PID] RETRY COUNT EXCEEDED"); - throw new TRoboClawException("[RoboClaw::setM2PID] RETRY COUNT EXCEEDED"); + CmdSetPid command(*this, kM2, p, i, d, qpps); + command.execute(); } void RoboClaw::stop() { @@ -1298,14 +1277,15 @@ void RoboClaw::writeByte2(uint8_t byte) { ssize_t result; do { result = ::write(device_port_, &byte, 1); -// RCUTILS_LOG_INFO("--> wrote: 0x%02X, result: %ld", byte, result); // #### -#if kDebugIO - if (result == 1) { - appendToLog("%02X ", byte); - } else { - appendToLog(">%02X< ", byte); + // RCUTILS_LOG_INFO("--> wrote: 0x%02X, result: %ld", byte, result); // + // #### + if (doLowLevelDebug_) { + if (result == 1) { + appendToWriteLog("%02X ", byte); + } else { + appendToWriteLog(">%02X< ", byte); + } } -#endif } while (result == -1 && errno == EAGAIN); @@ -1331,15 +1311,17 @@ void RoboClaw::writeN(bool sendCRC, uint8_t cnt, ...) { for (uint8_t i = 0; i < cnt; i++) { uint8_t byte = va_arg(marker, int); - if (i <= 1) { - snprintf(msg, sizeof(msg), - "RoboClaw::writeN] sendCRC: %d, cnt: %d, byte[%d]: %d", sendCRC, - cnt, i, byte); - RCUTILS_LOG_INFO("%s", msg); - if (i == 1) { - snprintf(msg, sizeof(msg), "[RoboClaw::writeN] command: %s", - commandNames_[byte]); + if (doLowLevelDebug_) { + if (i <= 1) { + snprintf(msg, sizeof(msg), + "RoboClaw::writeN2] sendCRC: %d, cnt: %d, byte[%d]: %d", + sendCRC, cnt, i, byte); RCUTILS_LOG_INFO("%s", msg); + if (i == 1) { + snprintf(msg, sizeof(msg), "[RoboClaw::writeN2] command: %s", + commandNames_[byte]); + RCUTILS_LOG_INFO("%s", msg); + } } } @@ -1355,10 +1337,11 @@ void RoboClaw::writeN(bool sendCRC, uint8_t cnt, ...) { uint8_t response = readByteWithTimeout(); if (response != 0xFF) { - snprintf(msg, sizeof(msg), - "[RoboClaw::writeN] Invalid ACK response, expected 0xFF but got " - "0x%02X", - response); + snprintf( + msg, sizeof(msg), + "[RoboClaw::writeN2] Invalid ACK response, expected 0xFF but got " + "0x%02X", + response); RCUTILS_LOG_ERROR("%s", msg); throw new TRoboClawException(msg); } @@ -1371,25 +1354,12 @@ void RoboClaw::writeN2(bool sendCRC, uint8_t cnt, ...) { uint16_t crc = 0; va_list marker; va_start(marker, cnt); - char msg[128]; // int origFlags = fcntl(device_port_, F_GETFL, 0); // fcntl(device_port_, F_SETFL, origFlags & ~O_NONBLOCK); for (uint8_t i = 0; i < cnt; i++) { uint8_t byte = va_arg(marker, int); - if (i <= 1) { - snprintf(msg, sizeof(msg), - "RoboClaw::writeN2] sendCRC: %d, cnt: %d, byte[%d]: %d", sendCRC, - cnt, i, byte); - RCUTILS_LOG_INFO("%s", msg); - if (i == 1) { - snprintf(msg, sizeof(msg), "[RoboClaw::writeN2] command: %s", - commandNames_[byte]); - RCUTILS_LOG_INFO("%s", msg); - } - } - updateCrc(crc, byte); writeByte2(byte); } @@ -1400,7 +1370,7 @@ void RoboClaw::writeN2(bool sendCRC, uint8_t cnt, ...) { writeByte2(crc >> 8); writeByte2(crc); - uint8_t response = readByteWithTimeout(); + uint8_t response = readByteWithTimeout2(); if (response != 0xFF) { char msg[128]; snprintf(