diff --git a/include/roboclaw.h b/include/roboclaw.h index d8ba4d6..739ac9c 100755 --- a/include/roboclaw.h +++ b/include/roboclaw.h @@ -31,6 +31,8 @@ class RoboClaw { public: + enum kMotor { kM1 = 0, kM2 = 1, kNone = 2 }; + // Bit positions used to build alarms. enum { kM1_OVER_CURRENT = 0x01, // Motor 1 current sense is too high. @@ -112,7 +114,7 @@ public: int32_t m2_quad_pulses_per_second, uint32_t m2_max_distance); - EncodeResult getEncoderCommandResult(WHICH_ENC command); + // EncodeResult getEncoderCommandResult(WHICH_ENC command); // Get RoboClaw error status bits. uint16_t getErrorStatus(); @@ -127,9 +129,13 @@ public: // Get the encoder value for motor 1. int32_t getM1Encoder(); + int8_t getM1EncoderStatus(); + // Get the encoder value for motor 2. int32_t getM2Encoder(); + int8_t getM2EncoderStatus(); + // Convenience structure to hold a pair of current values. typedef struct { float m1Current; @@ -141,7 +147,8 @@ public: TMotorCurrents getMotorCurrents(); - TPIDQ getPIDQ(WHICH_MOTOR whichMotor); + TPIDQ getPIDQM1(); + TPIDQ getPIDQM2(); float getTemperature(); @@ -157,7 +164,7 @@ public: // Get singleton instance of class. static RoboClaw *singleton(); - static void readSensorGroup(); + void readSensorGroup(); protected: // Write a stream of bytes to the device. @@ -172,36 +179,25 @@ private: // True => print byte values as they are read and written. bool doLowLevelDebug_; - EncodeResult cache_getEncoderCommandResult(WHICH_ENC command); - // Get RoboClaw error status bits. uint16_t cache_getErrorStatus(); // Get RoboClaw error status as a string. std::string cache_getErrorString(); - float cache_getLogicBatteryLevel(); - - // Get the encoder value for motor 1. - int32_t cache_getM1Encoder(); - int32_t cache_getM1Speed(); - // Get the encoder value for motor 2. - int32_t cache_getM2Encoder(); - int32_t cache_getM2Speed(); float cache_getMainBatteryLevel(); TMotorCurrents cache_getMotorCurrents(); - TPIDQ cache_getPIDQ(WHICH_MOTOR whichMotor); - float cache_getTemperature(); // Get velocity (speed) of a motor. - int32_t cache_getVelocity(WHICH_VELOCITY whichVelocity); + int32_t cache_getVelocityM1(); + int32_t cache_getVelocityM2(); static void sensorReadThread(); @@ -209,14 +205,12 @@ private: uint16_t error_status; std::string error_string; float logic_battery_level; - int32_t m1_encoder; EncodeResult m1_encoder_command_result; TPIDQ m1_pidq; int32_t m1_velocity; - int32_t m2_encoder; EncodeResult m2_encoder_command_result; TPIDQ m2_pidq; - int32_t m2_speed; + int32_t m2_velocity; float main_battery_level; int motor_alarms; TMotorCurrents motor_currents; @@ -224,7 +218,7 @@ private: std::chrono::system_clock::time_point last_sensor_read_time_; } SensorValueGroup; - static SensorValueGroup g_sensor_value_group_; + SensorValueGroup g_sensor_value_group_; typedef struct { unsigned long p1; @@ -322,10 +316,12 @@ private: unsigned long getUlongCommandResult(uint8_t command); - uint32_t getULongCont(uint16_t &crc); + uint32_t getULongCont2(uint16_t &crc); unsigned short get2ByteCommandResult(uint8_t command); + unsigned short get2ByteCommandResult2(uint8_t command); + // Open the RoboClaw USB port. void openPort(); @@ -365,8 +361,6 @@ private: char command_log_[256]; char response_log_[256]; - enum kMotor { kM1 = 0, kM2 = 1, kNone = 2 }; - class Cmd { public: void execute() { @@ -426,6 +420,9 @@ private: if (written > 0) { next_write_log_index_ += written; } + if ((next_write_log_index_ > 0) && (write_log_[0] == '8')) { + RCUTILS_LOG_INFO("Whoops!"); + } } void showLog() { @@ -498,6 +495,7 @@ private: responseCrc |= datum; if (responseCrc == crc) { version_ = version.str(); + roboclaw_.appendToReadLog(", RESULT: '%s'", version_.c_str()); roboclaw_.debug_log_.showLog(); return; } else { @@ -564,7 +562,7 @@ private: roboclaw_.updateCrc(crc, kGETERROR); roboclaw_.appendToWriteLog("ReadStatus: WROTE: "); roboclaw_.writeN2(false, 2, roboclaw_.portAddress_, kGETERROR); - unsigned short result = (unsigned short)roboclaw_.getULongCont(crc); + unsigned short result = (unsigned short)roboclaw_.getULongCont2(crc); uint16_t responseCrc = 0; uint16_t datum = roboclaw_.readByteWithTimeout2(); responseCrc = datum << 8; @@ -577,25 +575,200 @@ private: return; } else { RCUTILS_LOG_ERROR( - "[RoboClaw::cache_getErrorStatus] invalid CRC expected: 0x%02X, " + "[RoboClaw::CmdReadStatus] invalid CRC expected: 0x%02X, " "got: " "0x%02X", crc, responseCrc); } } catch (...) { - RCUTILS_LOG_ERROR( - "[RoboClaw::cache_getErrorStatus] Uncaught exception !!!"); + RCUTILS_LOG_ERROR("[RoboClaw::CmdReadStatus] Uncaught exception !!!"); } }; unsigned short &status_; }; + class CmdReadLogicBatteryVoltage : public Cmd { + public: + CmdReadLogicBatteryVoltage(RoboClaw &roboclaw, float &voltage) + : Cmd(roboclaw, "ReadLogicBatteryVoltage", kNone), voltage_(voltage) {} + void send() override { + try { + roboclaw_.appendToWriteLog("ReadLogicBatteryVoltage: WROTE: "); + float result = + ((float)roboclaw_.get2ByteCommandResult2(kGETLBATT)) / 10.0; + voltage_ = result; + roboclaw_.appendToReadLog(", RESULT: %f", result); + roboclaw_.debug_log_.showLog(); + return; + } catch (...) { + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadLogicBatteryVoltage] Uncaught exception !!!"); + } + } + + float &voltage_; + }; + + class CmdReadMainBatteryVoltage : public Cmd { + public: + CmdReadMainBatteryVoltage(RoboClaw &roboclaw, float &voltage) + : Cmd(roboclaw, "ReadLMainBatteryVoltage", kNone), voltage_(voltage) {} + void send() override { + try { + roboclaw_.appendToWriteLog("ReadLMainBatteryVoltage: WROTE: "); + float result = + ((float)roboclaw_.get2ByteCommandResult2(kGETMBATT)) / 10.0; + voltage_ = result; + roboclaw_.appendToReadLog(", RESULT: %f", result); + roboclaw_.debug_log_.showLog(); + return; + } catch (...) { + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadMainBatteryVoltage] Uncaught exception !!!"); + } + } + + float &voltage_; + }; + + class CmdReadEncoder : public Cmd { + public: + CmdReadEncoder(RoboClaw &roboclaw, kMotor motor, EncodeResult &encoder) + : Cmd(roboclaw, "ReadEncoder", motor), encoder_(encoder) {} + void send() override { + try { + roboclaw_.appendToWriteLog("ReadEncoder: encoder: %d (%s), WROTE: ", + motor_, motor_ == kM1 ? "M1" : "M2"); + + uint16_t crc = 0; + roboclaw_.updateCrc(crc, roboclaw_.portAddress_); + roboclaw_.updateCrc(crc, motor_ == kM1 ? kGETM1ENC : kGETM2ENC); + + roboclaw_.writeN2(false, 2, roboclaw_.portAddress_, + motor_ == kM1 ? kGETM1ENC : kGETM2ENC); + + uint8_t datum = roboclaw_.readByteWithTimeout2(); + encoder_.value |= datum << 24; + roboclaw_.updateCrc(crc, datum); + + datum = roboclaw_.readByteWithTimeout2(); + encoder_.value |= datum << 16; + roboclaw_.updateCrc(crc, datum); + + datum = roboclaw_.readByteWithTimeout2(); + encoder_.value |= datum << 8; + roboclaw_.updateCrc(crc, datum); + + datum = roboclaw_.readByteWithTimeout2(); + encoder_.value |= datum; + roboclaw_.updateCrc(crc, datum); + + datum = roboclaw_.readByteWithTimeout2(); + encoder_.status |= datum; + roboclaw_.updateCrc(crc, datum); + + uint16_t responseCrc = 0; + datum = roboclaw_.readByteWithTimeout2(); + responseCrc = datum << 8; + datum = roboclaw_.readByteWithTimeout2(); + responseCrc |= datum; + if (responseCrc != crc) { + RCUTILS_LOG_ERROR("[RoboClaw::CmdReadEncoder] Expected " + "CRC of: 0x%02X, but " + "got: 0x%02X", + int(crc), int(responseCrc)); + throw new TRoboClawException( + "[RoboClaw::CmdReadEncoder] INVALID CRC"); + } + + roboclaw_.appendToReadLog(", RESULT value: %d, status: %d", + encoder_.value, encoder_.status); + roboclaw_.debug_log_.showLog(); + return; + } catch (...) { + RCUTILS_LOG_ERROR("[RoboClaw::CmdReadEncoder] Uncaught exception !!!"); + } + } + + EncodeResult &encoder_; + }; + + class CmdReadMotorVelocityPIDQ : public Cmd { + public: + CmdReadMotorVelocityPIDQ(RoboClaw &roboclaw, kMotor motor, TPIDQ &pidq) + : Cmd(roboclaw, "ReadMotorVelocityPIDQ", motor), pidq_(pidq) {} + void send() override { + try { + roboclaw_.appendToWriteLog( + "ReadMotorVelocityPIDQ: motor: %d (%s), WROTE: ", motor_, + motor_ == kM1 ? "M1" : "M2"); + TPIDQ result; + uint16_t crc = 0; + roboclaw_.updateCrc(crc, roboclaw_.portAddress_); + roboclaw_.updateCrc(crc, motor_ == kM1 ? kGETM1PID : kGETM2PID); + + roboclaw_.writeN2(false, 2, roboclaw_.portAddress_, + motor_ == kM1 ? kGETM1PID : kGETM2PID); + result.p = (int32_t)roboclaw_.getULongCont2(crc) / 65536.0; + result.i = (int32_t)roboclaw_.getULongCont2(crc) / 65536.0; + result.d = (int32_t)roboclaw_.getULongCont2(crc) / 65536.0; + result.qpps = (int32_t)roboclaw_.getULongCont2(crc); + + uint16_t responseCrc = 0; + uint16_t datum = roboclaw_.readByteWithTimeout2(); + responseCrc = datum << 8; + datum = roboclaw_.readByteWithTimeout2(); + responseCrc |= datum; + roboclaw_.appendToReadLog( + ", RESULT: p: %3.4f, i: %3.4f, d: %3.4f, qpps: %d", result.p, + result.i, result.d, result.qpps); + roboclaw_.debug_log_.showLog(); + if (responseCrc == crc) { + pidq_ = result; + return; + } else { + RCUTILS_LOG_ERROR("[RoboClaw::CmdReadMotorVelocityPIDQ] invalid CRC " + "expected: 0x%2X, got: " + "0x%2X", + crc, responseCrc); + } + } catch (...) { + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadMotorVelocityPIDQ] Uncaught exception !!!"); + } + } + + TPIDQ &pidq_; + }; + + class CmdReadEncoderSpeed : public Cmd { + public: + CmdReadEncoderSpeed(RoboClaw &roboclaw, kMotor motor, int32_t &speed) + : Cmd(roboclaw, "ReadEncoderSpeed", motor), speed_(speed) {} + void send() override { + try { + roboclaw_.appendToWriteLog("ReadEncoderSpeed: motor: %d (%s), WROTE: ", + motor_, motor_ == kM1 ? "M1" : "M2"); + int32_t result = roboclaw_.getVelocityResult( + motor_ == kM1 ? kGETM1SPEED : kGETM2SPEED); + speed_ = result; + roboclaw_.appendToReadLog(", RESULT: %d", result); + roboclaw_.debug_log_.showLog(); + return; + } catch (...) { + RCUTILS_LOG_ERROR("[RoboClaw::CmdReadEncoderSpeed] Uncaught exception " + "!!!"); + } + } + + int32_t &speed_; + }; + friend class Cmd; // Make Cmd a friend class of RoboClaw protected: DebugLog debug_log_; - static const char *commandNames_[]; static const char *motorNames_[]; }; diff --git a/launch/ros2_roboclaw_driver.launch.py b/launch/ros2_roboclaw_driver.launch.py index e0eebeb..9614da5 100644 --- a/launch/ros2_roboclaw_driver.launch.py +++ b/launch/ros2_roboclaw_driver.launch.py @@ -6,6 +6,7 @@ from launch import LaunchDescription import launch_ros.actions from launch_ros.actions import Node + def generate_launch_description(): # The following 'my_package_name' should be changed to the # package name in your robot workspace that will contain @@ -37,7 +38,8 @@ def generate_launch_description(): # file for more information. with open(configFilePath, 'r') as file: - configParams = yaml.safe_load(file)['motor_driver_node']['ros__parameters'] + configParams = yaml.safe_load( + file)['motor_driver_node']['ros__parameters'] ld = LaunchDescription() @@ -47,7 +49,7 @@ def generate_launch_description(): executable='ros2_roboclaw_driver_node', package='ros2_roboclaw_driver', parameters=[configParams], - #prefix=['xterm -e gdb -ex run --args'], + # prefix=['xterm -e gdb -ex run --args'], respawn=True, output='screen') ld.add_action(motor_driver_node) diff --git a/src/motor_driver_node.cpp b/src/motor_driver_node.cpp index 92681db..60672af 100644 --- a/src/motor_driver_node.cpp +++ b/src/motor_driver_node.cpp @@ -2,82 +2,80 @@ #include #include "motor_driver.h" -#include "ros2_roboclaw_driver/msg/robo_claw_status.hpp" #include "roboclaw.h" +#include "ros2_roboclaw_driver/msg/robo_claw_status.hpp" -int main(int argc, char *argv[]) -{ +int main(int argc, char *argv[]) { rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("ros2_roboclaw_driver_node"); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared("ros2_roboclaw_driver_node"); MotorDriver &motorDriver = MotorDriver::singleton(); motorDriver.onInit(node); auto qos = rclcpp::QoS( - rclcpp::QoSInitialization( - RMW_QOS_POLICY_HISTORY_KEEP_LAST, - 10)); + rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 10)); qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); qos.avoid_ros_namespace_conventions(false); std::string statusTopicName; - node->declare_parameter("roboclaw_status_topic", "roboclaw_status"); + node->declare_parameter("roboclaw_status_topic", + "roboclaw_status"); node->get_parameter("roboclaw_status_topic", statusTopicName); - RCUTILS_LOG_INFO("[motor_driver_node] roboclaw_status_topic: %s", statusTopicName.c_str()); + RCUTILS_LOG_INFO("[motor_driver_node] roboclaw_status_topic: %s", + statusTopicName.c_str()); - rclcpp::Publisher::SharedPtr statusPublisher = node->create_publisher(statusTopicName, qos); + rclcpp::Publisher::SharedPtr + statusPublisher = + node->create_publisher( + statusTopicName, qos); ros2_roboclaw_driver::msg::RoboClawStatus roboClawStatus; rclcpp::WallRate loop_rate(20); - while (rclcpp::ok()) - { - try - { - roboClawStatus.logic_battery_voltage = RoboClaw::singleton()->getLogicBatteryLevel(); - roboClawStatus.main_battery_voltage = RoboClaw::singleton()->getMainBatteryLevel(); - RoboClaw::TMotorCurrents motorCurrents = RoboClaw::singleton()->getMotorCurrents(); + while (rclcpp::ok()) { + try { + roboClawStatus.logic_battery_voltage = + RoboClaw::singleton()->getLogicBatteryLevel(); + roboClawStatus.main_battery_voltage = + RoboClaw::singleton()->getMainBatteryLevel(); + RoboClaw::TMotorCurrents motorCurrents = + RoboClaw::singleton()->getMotorCurrents(); roboClawStatus.m1_motor_current = motorCurrents.m1Current; roboClawStatus.m2_motor_current = motorCurrents.m2Current; - RoboClaw::TPIDQ pidq = RoboClaw::singleton()->getPIDQ(RoboClaw::kGETM1PID); - roboClawStatus.m1_p = pidq.p / 65536.0; - roboClawStatus.m1_i = pidq.i / 65536.0; - roboClawStatus.m1_d = pidq.d / 65536.0; + RoboClaw::TPIDQ pidq = RoboClaw::singleton()->getPIDQM1(); + + roboClawStatus.m1_p = pidq.p; + roboClawStatus.m1_i = pidq.i; + roboClawStatus.m1_d = pidq.d; roboClawStatus.m1_qpps = pidq.qpps; - pidq = RoboClaw::singleton()->getPIDQ(RoboClaw::kGETM2PID); - roboClawStatus.m2_p = pidq.p / 65536.0; - roboClawStatus.m2_i = pidq.i / 65536.0; - roboClawStatus.m2_d = pidq.d / 65536.0; + pidq = RoboClaw::singleton()->getPIDQM2(); + roboClawStatus.m2_p = pidq.p; + roboClawStatus.m2_i = pidq.i; + roboClawStatus.m2_d = pidq.d; roboClawStatus.m2_qpps = pidq.qpps; roboClawStatus.temperature = RoboClaw::singleton()->getTemperature(); - { - RoboClaw::EncodeResult encoder = RoboClaw::singleton()->getEncoderCommandResult(RoboClaw::kGETM1ENC); - roboClawStatus.m1_encoder_value = encoder.value; - roboClawStatus.m1_encoder_status = encoder.status; - } + roboClawStatus.m1_encoder_value = RoboClaw::singleton()->getM1Encoder(); + roboClawStatus.m1_encoder_status = + RoboClaw::singleton()->getM1EncoderStatus(); + roboClawStatus.m2_encoder_value = RoboClaw::singleton()->getM2Encoder(); + roboClawStatus.m2_encoder_status = + RoboClaw::singleton()->getM2EncoderStatus(); - { - RoboClaw::EncodeResult encoder = RoboClaw::singleton()->getEncoderCommandResult(RoboClaw::kGETM2ENC); - roboClawStatus.m2_encoder_value = encoder.value; - roboClawStatus.m2_encoder_status = encoder.status; - } - - roboClawStatus.m1_current_speed = RoboClaw::singleton()->getVelocity(RoboClaw::kGETM1SPEED); - roboClawStatus.m2_current_speed = RoboClaw::singleton()->getVelocity(RoboClaw::kGETM2SPEED); + roboClawStatus.m1_current_speed = + RoboClaw::singleton()->getVelocity(RoboClaw::kGETM1SPEED); + roboClawStatus.m2_current_speed = + RoboClaw::singleton()->getVelocity(RoboClaw::kGETM2SPEED); roboClawStatus.error_string = RoboClaw::singleton()->getErrorString(); statusPublisher->publish(roboClawStatus); - } - catch (RoboClaw::TRoboClawException *e) - { + } catch (RoboClaw::TRoboClawException *e) { RCUTILS_LOG_ERROR("[motor_driver_node] Exception: %s", e->what()); - } - catch (...) - { + } catch (...) { RCUTILS_LOG_ERROR("[motor_driver_node] Uncaught exception !!!"); } diff --git a/src/roboclaw.cpp b/src/roboclaw.cpp index 8bc26c3..2c0d486 100755 --- a/src/roboclaw.cpp +++ b/src/roboclaw.cpp @@ -1,5 +1,6 @@ #include "roboclaw.h" +#include #include #include #include @@ -17,262 +18,6 @@ #include "ros2_roboclaw_driver/srv/reset_encoders.h" const char *RoboClaw::motorNames_[] = {"M1", "M2", "NONE"}; -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, @@ -325,59 +70,6 @@ void RoboClaw::doMixedSpeedAccelDist(uint32_t accel_quad_pulses_per_second, ); } -RoboClaw::EncodeResult RoboClaw::getEncoderCommandResult(WHICH_ENC command) { - if (command == kGETM1ENC) { - return g_sensor_value_group_.m1_encoder_command_result; - } else { - return g_sensor_value_group_.m2_encoder_command_result; - } -} - -RoboClaw::EncodeResult -RoboClaw::cache_getEncoderCommandResult(WHICH_ENC command) { - uint16_t crc = 0; - updateCrc(crc, portAddress_); - updateCrc(crc, command); - - writeN(false, 2, portAddress_, command); - EncodeResult result = {0, 0}; - uint8_t datum = readByteWithTimeout(); - result.value |= datum << 24; - updateCrc(crc, datum); - - datum = readByteWithTimeout(); - result.value |= datum << 16; - updateCrc(crc, datum); - - datum = readByteWithTimeout(); - result.value |= datum << 8; - updateCrc(crc, datum); - - datum = readByteWithTimeout(); - result.value |= datum; - updateCrc(crc, datum); - - datum = readByteWithTimeout(); - result.status |= datum; - updateCrc(crc, datum); - - uint16_t responseCrc = 0; - datum = readByteWithTimeout(); - responseCrc = datum << 8; - datum = readByteWithTimeout(); - responseCrc |= datum; - if (responseCrc == crc) { - return result; - } - - RCUTILS_LOG_ERROR( - "[RoboClaw::cache_getEncoderCommandResult] Expected CRC of: 0x%02X, but " - "got: 0x%02X", - int(crc), int(responseCrc)); - throw new TRoboClawException( - "[RoboClaw::cache_getEncoderCommandResult] INVALID CRC"); -} - uint16_t RoboClaw::getErrorStatus() { return g_sensor_value_group_.error_status; } @@ -477,54 +169,6 @@ float RoboClaw::getLogicBatteryLevel() { return g_sensor_value_group_.logic_battery_level; } -float RoboClaw::cache_getLogicBatteryLevel() { - int retry; - - for (retry = 0; retry < maxCommandRetries_; retry++) { - try { - float result = ((float)get2ByteCommandResult(kGETLBATT)) / 10.0; - return result; - } catch (TRoboClawException *e) { - RCUTILS_LOG_ERROR( - "[RoboClaw::cache_getLogicBatteryLevel] Exception: %s, retry " - "number: %d", - e->what(), retry); - } catch (...) { - RCUTILS_LOG_ERROR( - "[RoboClaw::cache_getLogicBatteryLevel] Uncaught exception !!!"); - } - } - - RCUTILS_LOG_ERROR( - "[RoboClaw::cache_getLogicBatteryLevel] RETRY COUNT EXCEEDED"); - throw new TRoboClawException( - "[RoboClaw::cache_getLogicBatteryLevel] RETRY COUNT EXCEEDED"); -} - -int32_t RoboClaw::getM1Encoder() { return g_sensor_value_group_.m1_encoder; } - -int32_t RoboClaw::cache_getM1Encoder() { - int retry; - - for (retry = 0; retry < maxCommandRetries_; retry++) { - try { - EncodeResult result = getEncoderCommandResult(kGETM1ENC); - return result.value; - } catch (TRoboClawException *e) { - RCUTILS_LOG_ERROR( - "[RoboClaw::cache_getM1Encoder] Exception: %s, retry number: %d", - e->what(), retry); - } catch (...) { - RCUTILS_LOG_ERROR( - "[RoboClaw::cache_getM1Encoder] Uncaught exception !!!"); - } - } - - RCUTILS_LOG_ERROR("[RoboClaw::cache_getM1Encoder] RETRY COUNT EXCEEDED"); - throw new TRoboClawException( - "[RoboClaw::cache_getM1Encoder] RETRY COUNT EXCEEDED"); -} - float RoboClaw::getMainBatteryLevel() { return g_sensor_value_group_.main_battery_level; } @@ -585,6 +229,40 @@ unsigned short RoboClaw::get2ByteCommandResult(uint8_t command) { } } +// ### change result type to uint16_t +unsigned short RoboClaw::get2ByteCommandResult2(uint8_t command) { + uint16_t crc = 0; + updateCrc(crc, portAddress_); + updateCrc(crc, command); + + writeN2(false, 2, portAddress_, command); + unsigned short result = 0; + uint8_t datum = readByteWithTimeout2(); + result |= datum << 8; + updateCrc(crc, datum); + + datum = readByteWithTimeout2(); + result |= datum; + updateCrc(crc, datum); + + uint16_t responseCrc = 0; + datum = readByteWithTimeout2(); + responseCrc = datum << 8; + datum = readByteWithTimeout2(); + responseCrc |= datum; + if (responseCrc == crc) { + return result; + } else { + RCUTILS_LOG_ERROR( + "[RoboClaw::get2ByteCommandResult2] invalid CRC expected: " + "0x%02X, got: 0x%02X", + crc, responseCrc); + throw new TRoboClawException( + "[RoboClaw::get2ByteCommandResult] INVALID CRC"); + return 0; + } +} + RoboClaw::TMotorCurrents RoboClaw::getMotorCurrents() { return g_sensor_value_group_.motor_currents; } @@ -636,55 +314,18 @@ RoboClaw::TMotorCurrents RoboClaw::cache_getMotorCurrents() { "[RoboClaw::cache_getMotorCurrents] RETRY COUNT EXCEEDED"); } -RoboClaw::TPIDQ RoboClaw::getPIDQ(WHICH_MOTOR whichMotor) { - if (whichMotor == kGETM1PID) { - return g_sensor_value_group_.m1_pidq; - } else { - return g_sensor_value_group_.m2_pidq; - } +RoboClaw::TPIDQ RoboClaw::getPIDQM1() { + TPIDQ result; + CmdReadMotorVelocityPIDQ cmd(*this, kM1, result); + cmd.execute(); + return result; } -RoboClaw::TPIDQ RoboClaw::cache_getPIDQ(WHICH_MOTOR whichMotor) { - int retry; - - for (retry = 0; retry < maxCommandRetries_; retry++) { - TPIDQ result; - try { - uint16_t crc = 0; - updateCrc(crc, portAddress_); - updateCrc(crc, whichMotor); - - writeN(false, 2, portAddress_, whichMotor); - result.p = (int32_t)getULongCont(crc); - result.i = (int32_t)getULongCont(crc); - result.d = (int32_t)getULongCont(crc); - result.qpps = (int32_t)getULongCont(crc); - - uint16_t responseCrc = 0; - uint16_t datum = readByteWithTimeout(); - responseCrc = datum << 8; - datum = readByteWithTimeout(); - responseCrc |= datum; - if (responseCrc == crc) { - return result; - } else { - RCUTILS_LOG_ERROR( - "[RoboClaw::cache_getPIDQ] invalid CRC expected: 0x%2X, got: " - "0x%2X", - crc, responseCrc); - } - } catch (TRoboClawException *e) { - RCUTILS_LOG_ERROR( - "[RoboClaw::cache_getPIDQ] Exception: %s, retry number: %d", - e->what(), retry); - } catch (...) { - RCUTILS_LOG_ERROR("[RoboClaw::cache_getPIDQ] Uncaught exception !!!"); - } - } - - RCUTILS_LOG_ERROR("[RoboClaw::cache_getPIDQ] RETRY COUNT EXCEEDED"); - throw new TRoboClawException( - "[RoboClaw::cache_getPIDQ] RETRY COUNT EXCEEDED"); +RoboClaw::TPIDQ RoboClaw::getPIDQM2() { + TPIDQ result; + CmdReadMotorVelocityPIDQ cmd(*this, kM2, result); + cmd.execute(); + return result; } float RoboClaw::getTemperature() { return g_sensor_value_group_.temperature; } @@ -771,18 +412,18 @@ unsigned long RoboClaw::getUlongCommandResult(uint8_t command) { return 0; } -uint32_t RoboClaw::getULongCont(uint16_t &crc) { +uint32_t RoboClaw::getULongCont2(uint16_t &crc) { uint32_t result = 0; - uint8_t datum = readByteWithTimeout(); + uint8_t datum = readByteWithTimeout2(); result |= datum << 24; updateCrc(crc, datum); - datum = readByteWithTimeout(); + datum = readByteWithTimeout2(); result |= datum << 16; updateCrc(crc, datum); - datum = readByteWithTimeout(); + datum = readByteWithTimeout2(); result |= datum << 8; updateCrc(crc, datum); - datum = readByteWithTimeout(); + datum = readByteWithTimeout2(); result |= datum; updateCrc(crc, datum); return result; @@ -792,29 +433,22 @@ int32_t RoboClaw::getVelocity(WHICH_VELOCITY whichVelocity) { if (whichVelocity == kGETM1SPEED) { return g_sensor_value_group_.m1_velocity; } else { - return g_sensor_value_group_.m2_speed; + return g_sensor_value_group_.m2_velocity; } } -int32_t RoboClaw::cache_getVelocity(WHICH_VELOCITY whichVelocity) { - int retry; +int32_t RoboClaw::cache_getVelocityM1() { + int32_t result; + CmdReadEncoderSpeed cmd(*this, kM1, result); + cmd.execute(); + return result; +} - for (retry = 0; retry < maxCommandRetries_; retry++) { - try { - uint32_t result = getVelocityResult(whichVelocity); - return result; - } catch (TRoboClawException *e) { - RCUTILS_LOG_ERROR( - "[RoboClaw::cache_getVelocity] Exception: %s, retry number: %d", - e->what(), retry); - } catch (...) { - RCUTILS_LOG_ERROR("[RoboClaw::cache_getVelocity] Uncaught exception !!!"); - } - } - - RCUTILS_LOG_ERROR("RoboClaw::cache_getVelocity] RETRY COUNT EXCEEDED"); - throw new TRoboClawException( - "[RoboClaw::cache_getVelocity] RETRY COUNT EXCEEDED"); +int32_t RoboClaw::cache_getVelocityM2() { + int32_t result; + CmdReadEncoderSpeed cmd(*this, kM2, result); + cmd.execute(); + return result; } int32_t RoboClaw::getVelocityResult(uint8_t command) { @@ -822,33 +456,33 @@ int32_t RoboClaw::getVelocityResult(uint8_t command) { updateCrc(crc, portAddress_); updateCrc(crc, command); - writeN(false, 2, portAddress_, command); + writeN2(false, 2, portAddress_, command); int32_t result = 0; - uint8_t datum = readByteWithTimeout(); + uint8_t datum = readByteWithTimeout2(); result |= datum << 24; updateCrc(crc, datum); - datum = readByteWithTimeout(); + datum = readByteWithTimeout2(); result |= datum << 16; updateCrc(crc, datum); - datum = readByteWithTimeout(); + datum = readByteWithTimeout2(); result |= datum << 8; updateCrc(crc, datum); - datum = readByteWithTimeout(); + datum = readByteWithTimeout2(); result |= datum; updateCrc(crc, datum); - uint8_t direction = readByteWithTimeout(); + uint8_t direction = readByteWithTimeout2(); updateCrc(crc, direction); if (direction != 0) result = -result; uint16_t responseCrc = 0; - datum = readByteWithTimeout(); + datum = readByteWithTimeout2(); responseCrc = datum << 8; - datum = readByteWithTimeout(); + datum = readByteWithTimeout2(); responseCrc |= datum; if (responseCrc == crc) { return result; @@ -862,28 +496,20 @@ int32_t RoboClaw::getVelocityResult(uint8_t command) { return 0; } -int32_t RoboClaw::getM2Encoder() { return g_sensor_value_group_.m2_encoder; } +int32_t RoboClaw::getM1Encoder() { + return g_sensor_value_group_.m1_encoder_command_result.value; +} -int32_t RoboClaw::cache_getM2Encoder() { - int retry; +int8_t RoboClaw::getM1EncoderStatus() { + return g_sensor_value_group_.m1_encoder_command_result.status; +} - for (retry = 0; retry < maxCommandRetries_; retry++) { - try { - EncodeResult result = getEncoderCommandResult(kGETM2ENC); - return result.value; - } catch (TRoboClawException *e) { - RCUTILS_LOG_ERROR( - "[RoboClaw::cache_getM2Encoder] Exception: %s, retry number: %d", - e->what(), retry); - } catch (...) { - RCUTILS_LOG_ERROR( - "[RoboClaw::cache_getM2Encoder] Uncaught exception !!!"); - } - } +int32_t RoboClaw::getM2Encoder() { + return g_sensor_value_group_.m2_encoder_command_result.value; +} - RCUTILS_LOG_ERROR("[RoboClaw::cache_getM2Encoder] RETRY COUNT EXCEEDED"); - throw new TRoboClawException( - "[RoboClaw::cache_getM2Encoder] RETRY COUNT EXCEEDED"); +int8_t RoboClaw::getM2EncoderStatus() { + return g_sensor_value_group_.m2_encoder_command_result.status; } std::string RoboClaw::getVersion() { @@ -1104,24 +730,37 @@ uint8_t RoboClaw::readByteWithTimeout2() { void RoboClaw::readSensorGroup() { if (singleton() != nullptr) { + TPIDQ m1_read_velocity_pidq_result; + TPIDQ m2_read_velocity_pidq_result; + CmdReadMotorVelocityPIDQ cmd_m1_read_motor_velocity_pidq( + *this, kM1, m1_read_velocity_pidq_result); + cmd_m1_read_motor_velocity_pidq.execute(); + CmdReadMotorVelocityPIDQ cmd_m2_read_motor_velocity_pidq( + *this, kM2, m2_read_velocity_pidq_result); + cmd_m2_read_motor_velocity_pidq.execute(); + float logic_battery_level = 0.0; + CmdReadLogicBatteryVoltage cmd_logic_battery(*this, logic_battery_level); + cmd_logic_battery.execute(); + float main_battery_level = 0.0; + CmdReadMainBatteryVoltage cmd_main_battery(*this, main_battery_level); + cmd_main_battery.execute(); + EncodeResult m1_encoder_command_result; + EncodeResult m2_encoder_command_result; + CmdReadEncoder m1_read_encoder_cmd(*this, kM1, m1_encoder_command_result); + m1_read_encoder_cmd.execute(); + CmdReadEncoder m2_read_encoder_cmd(*this, kM2, m2_encoder_command_result); + m2_read_encoder_cmd.execute(); + + int32_t m1_speed = singleton()->cache_getVelocityM1(); + int32_t m2_speed = singleton()->cache_getVelocityM2(); g_sensor_value_group_.error_status = singleton()->cache_getErrorStatus(); g_sensor_value_group_.error_string = singleton()->cache_getErrorString(); - g_sensor_value_group_.logic_battery_level = - singleton()->cache_getLogicBatteryLevel(); - g_sensor_value_group_.m1_encoder = singleton()->cache_getM1Encoder(); - g_sensor_value_group_.m1_encoder_command_result = - singleton()->cache_getEncoderCommandResult(kGETM1ENC); - g_sensor_value_group_.m1_pidq = singleton()->cache_getPIDQ(kGETM1PID); - g_sensor_value_group_.m1_velocity = - singleton()->cache_getVelocity(RoboClaw::kGETM1SPEED); - g_sensor_value_group_.m2_encoder = singleton()->cache_getM2Encoder(); - g_sensor_value_group_.m2_encoder_command_result = - singleton()->cache_getEncoderCommandResult(kGETM2ENC); - g_sensor_value_group_.m1_pidq = singleton()->cache_getPIDQ(kGETM2PID); - g_sensor_value_group_.m2_speed = - singleton()->cache_getVelocity(RoboClaw::kGETM2SPEED); - g_sensor_value_group_.main_battery_level = - singleton()->cache_getMainBatteryLevel(); + g_sensor_value_group_.logic_battery_level = logic_battery_level; + g_sensor_value_group_.m1_encoder_command_result = m1_encoder_command_result; + g_sensor_value_group_.m1_velocity = m1_speed; + g_sensor_value_group_.m2_encoder_command_result = m2_encoder_command_result; + g_sensor_value_group_.m2_velocity = m2_speed; + g_sensor_value_group_.main_battery_level = main_battery_level; // Call getMotorCurrents before getMotorAlarms; g_sensor_value_group_.motor_currents = @@ -1280,20 +919,6 @@ void RoboClaw::writeN(bool sendCRC, uint8_t cnt, ...) { for (uint8_t i = 0; i < cnt; i++) { uint8_t byte = va_arg(marker, int); - 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); - } - } - } - updateCrc(crc, byte); writeByte(byte); } @@ -1357,5 +982,4 @@ void RoboClaw::writeN2(bool sendCRC, uint8_t cnt, ...) { RoboClaw *RoboClaw::singleton() { return g_singleton; } -RoboClaw::SensorValueGroup RoboClaw::g_sensor_value_group_; RoboClaw *RoboClaw::g_singleton = nullptr; \ No newline at end of file