From b9c51591c04247962ab4a1f8cdec93eeb0c23089 Mon Sep 17 00:00:00 2001 From: Michael Wimble Date: Sat, 22 Mar 2025 13:01:53 -0700 Subject: [PATCH] Broke out all Cmd class files into their own files for better organization and readability. --- config/motor_driver.yaml | 2 +- include/roboclaw.h | 525 +----------------- include/roboclaw_cmd.h | 47 ++ ...buffered_m1m2_drive_speed_accel_distance.h | 48 ++ include/roboclaw_cmd_read_encoder.h | 69 +++ include/roboclaw_cmd_read_encoder_speed.h | 27 + include/roboclaw_cmd_read_firmware_version.h | 65 +++ .../roboclaw_cmd_read_logic_battery_voltage.h | 25 + .../roboclaw_cmd_read_main_battery_voltage.h | 25 + include/roboclaw_cmd_read_motor_currents.h | 51 ++ .../roboclaw_cmd_read_motor_velocity_pidq.h | 54 ++ include/roboclaw_cmd_read_status.h | 38 ++ include/roboclaw_cmd_read_temperature.h | 49 ++ include/roboclaw_cmd_set_encoder_value.h | 27 + include/roboclaw_cmd_set_pid.h | 29 + src/roboclaw.cpp | 12 + 16 files changed, 593 insertions(+), 500 deletions(-) create mode 100644 include/roboclaw_cmd.h create mode 100644 include/roboclaw_cmd_do_buffered_m1m2_drive_speed_accel_distance.h create mode 100644 include/roboclaw_cmd_read_encoder.h create mode 100644 include/roboclaw_cmd_read_encoder_speed.h create mode 100644 include/roboclaw_cmd_read_firmware_version.h create mode 100644 include/roboclaw_cmd_read_logic_battery_voltage.h create mode 100644 include/roboclaw_cmd_read_main_battery_voltage.h create mode 100644 include/roboclaw_cmd_read_motor_currents.h create mode 100644 include/roboclaw_cmd_read_motor_velocity_pidq.h create mode 100644 include/roboclaw_cmd_read_status.h create mode 100644 include/roboclaw_cmd_read_temperature.h create mode 100644 include/roboclaw_cmd_set_encoder_value.h create mode 100644 include/roboclaw_cmd_set_pid.h diff --git a/config/motor_driver.yaml b/config/motor_driver.yaml index 4c9c026..fedd086 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/ttyAMA1" + device_name: "/dev/ttyACM2" baud_rate: 38400 # The assigned port for the device (as configured on the RoboClaw). diff --git a/include/roboclaw.h b/include/roboclaw.h index 2064c8e..db2357a 100755 --- a/include/roboclaw.h +++ b/include/roboclaw.h @@ -12,6 +12,19 @@ #include #include "ros2_roboclaw_driver/srv/reset_encoders.hpp" +// #include "roboclaw_cmd.h" +// #include "roboclaw_cmd_do_buffered_m1m2_drive_speed_accel_distance.h" +// #include "roboclaw_cmd_read_encoder.h" +// #include "roboclaw_cmd_read_encoder_speed.h" +// #include "roboclaw_cmd_read_firmware_version.h" +// #include "roboclaw_cmd_read_logic_battery_voltage.h" +// #include "roboclaw_cmd_read_main_battery_voltage.h" +// #include "roboclaw_cmd_read_motor_currents.h" +// #include "roboclaw_cmd_read_motor_velocity_pidq.h" +// #include "roboclaw_cmd_read_status.h" +// #include "roboclaw_cmd_read_temperature.h" +// #include "roboclaw_cmd_set_encoder_value.h" +// #include "roboclaw_cmd_set_pid.h" /* There are possibly several clients that want to read sensor data, such as encoder values, motor speeds, etc. Each time an actual read @@ -342,50 +355,6 @@ class RoboClaw { char command_log_[256]; char response_log_[256]; - class Cmd { - public: - void execute() { - for (int retry = 0; retry < 3 /*### maxCommandRetries_*/; retry++) { - try { - std::lock_guard lock( - RoboClaw::buffered_command_mutex_); // Lock the mutex - send(); - roboclaw_.debug_log_.showLog(); - return; - } catch (TRoboClawException *e) { - roboclaw_.debug_log_.showLog(); - RCUTILS_LOG_ERROR( - "[RoboClaw::Cmd::execute] Exception: %s, retry number: %d", - e->what(), retry); - } catch (...) { - roboclaw_.debug_log_.showLog(); - RCUTILS_LOG_ERROR("[RoboClaw::Cmd::execute] Uncaught exception !!!"); - } - } - - roboclaw_.debug_log_.showLog(); - RCUTILS_LOG_ERROR("[RoboClaw::Cmd::execute] RETRY COUNT EXCEEDED"); - throw new TRoboClawException( - "[RoboClaw::Cmd::execute] RETRY COUNT EXCEEDED"); - } - - virtual void send() = 0; // Declare send as a pure virtual function - - protected: - Cmd(RoboClaw &roboclaw, const char *name, const kMotor motor) - : motor_(motor), roboclaw_(roboclaw) { - strncpy(name_, name, sizeof(name_)); - name_[sizeof(name_) - 1] = '\0'; // Ensure null-termination - } - - kMotor motor_; - RoboClaw &roboclaw_; - char name_[32]; - - private: - Cmd() = delete; // Disallow default constructor - }; - class DebugLog { public: DebugLog() : next_read_log_index_(0), next_write_log_index_(0) {} @@ -428,461 +397,19 @@ class RoboClaw { uint16_t next_write_log_index_; }; - class CmdSetPid : public Cmd { - public: - CmdSetPid(RoboClaw &roboclaw, kMotor motor, float p, float i, float d, - uint32_t qpps) - : Cmd(roboclaw, "SetPid", motor), p_(p), i_(i), d_(d), qpps_(qpps) {} - - void send() override { - 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); - roboclaw_.writeN2(true, 18, roboclaw_.portAddress_, - motor_ == kM1 ? kSETM1PID : kSETM2PID, SetDWORDval(kd), - SetDWORDval(kp), SetDWORDval(ki), SetDWORDval(qpps_)); - } - - float p_; - float i_; - float d_; - 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_.appendToReadLog(", RESULT: '%s'", version_.c_str()); - return; - } else { - RCUTILS_LOG_ERROR( - "[RoboClaw::CmdReadFirmwareVersion] invalid " - "CRC expected: 0x%02X, " - "got: 0x%02X", - crc, responseCrc); - throw new TRoboClawException( - "[RoboClaw::getVersion] 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_); - try { - roboclaw_.writeN2(true, 6, roboclaw_.portAddress_, - motor_ == kM1 ? kSETM1ENCODER : kSETM2ENCODER, - SetDWORDval(value_)); - return; - } catch (...) { - RCUTILS_LOG_ERROR( - "[RoboClaw::CmdSetEncoderValue] Uncaught exception !!!"); - } - } - - long value_; - }; - - class CmdReadStatus : public Cmd { - public: - CmdReadStatus(RoboClaw &roboclaw, uint16_t &status) - : Cmd(roboclaw, "ReadStatus", kNone), status_(status) {} - void send() override { - try { - uint16_t crc = 0; - roboclaw_.updateCrc(crc, roboclaw_.portAddress_); - roboclaw_.updateCrc(crc, kGETERROR); - roboclaw_.appendToWriteLog("ReadStatus: WROTE: "); - roboclaw_.writeN2(false, 2, roboclaw_.portAddress_, kGETERROR); - status_ = (unsigned short)roboclaw_.getULongCont2(crc); - uint16_t responseCrc = 0; - uint16_t datum = roboclaw_.readByteWithTimeout2(); - responseCrc = datum << 8; - datum = roboclaw_.readByteWithTimeout2(); - responseCrc |= datum; - if (responseCrc == crc) { - roboclaw_.appendToReadLog(", RESULT: %04X", status_); - return; - } else { - RCUTILS_LOG_ERROR( - "[RoboClaw::CmdReadStatus] invalid CRC expected: 0x%02X, " - "got: " - "0x%02X", - crc, responseCrc); - } - } catch (...) { - RCUTILS_LOG_ERROR("[RoboClaw::CmdReadStatus] Uncaught exception !!!"); - } - }; - - uint16_t &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); - 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); - 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); - 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); - 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"); - speed_ = roboclaw_.getVelocityResult(motor_ == kM1 ? kGETM1SPEED - : kGETM2SPEED); - roboclaw_.appendToReadLog(", RESULT: %d", speed_); - return; - } catch (...) { - RCUTILS_LOG_ERROR( - "[RoboClaw::CmdReadEncoderSpeed] Uncaught exception " - "!!!"); - } - } - - int32_t &speed_; - }; - - class CmdReadMotorCurrents : public Cmd { - public: - CmdReadMotorCurrents(RoboClaw &roboclaw, TMotorCurrents &motorCurrents) - : Cmd(roboclaw, "ReadMotorCurrents", kNone), - motorCurrents_(motorCurrents) {} - void send() override { - try { - roboclaw_.appendToWriteLog("ReadMotorCurrents: WROTE: "); - - unsigned long currentPair = - roboclaw_.getUlongCommandResult2(kGETCURRENTS); - motorCurrents_.m1Current = ((int16_t)(currentPair >> 16)) * 0.010; - motorCurrents_.m2Current = ((int16_t)(currentPair & 0xFFFF)) * 0.010; - roboclaw_.appendToReadLog( - ", RESULT m1 current: %3.4f, m2 current: %3.4f", - motorCurrents_.m1Current, motorCurrents_.m2Current); - - if (motorCurrents_.m1Current > roboclaw_.maxM1Current_) { - roboclaw_.motorAlarms_ |= kM1_OVER_CURRENT_ALARM; - RCUTILS_LOG_ERROR( - "[RoboClaw::CmdReadMotorCurrents] Motor 1 over current. Max " - "allowed: %6.3f, found: %6.3f", - roboclaw_.maxM1Current_, motorCurrents_.m1Current); - roboclaw_.stop(); - } else { - roboclaw_.motorAlarms_ &= ~kM1_OVER_CURRENT_ALARM; - } - - if (motorCurrents_.m2Current > roboclaw_.maxM2Current_) { - roboclaw_.motorAlarms_ |= kM2_OVER_CURRENT_ALARM; - RCUTILS_LOG_ERROR( - "[RoboClaw::CmdReadMotorCurrents] Motor 2 over current. Max " - "allowed: %6.3f, found: %6.3f", - roboclaw_.maxM2Current_, motorCurrents_.m2Current); - roboclaw_.stop(); - } else { - roboclaw_.motorAlarms_ &= ~kM2_OVER_CURRENT_ALARM; - } - } catch (...) { - RCUTILS_LOG_ERROR( - "[RoboClaw::CmdReadMotorCurrents] Uncaught exception !!!"); - } - } - - TMotorCurrents &motorCurrents_; - }; - - class CmdReadTemperature : public Cmd { - public: - CmdReadTemperature(RoboClaw &roboclaw, float &temperature) - : Cmd(roboclaw, "ReadTemperature", kNone), temperature_(temperature) {} - void send() override { - try { - roboclaw_.appendToWriteLog("ReadTemperature: WROTE: "); - uint16_t crc = 0; - roboclaw_.updateCrc(crc, roboclaw_.portAddress_); - roboclaw_.updateCrc(crc, kGETTEMPERATURE); - roboclaw_.writeN2(false, 2, roboclaw_.portAddress_, kGETTEMPERATURE); - uint16_t result = 0; - uint8_t datum = roboclaw_.readByteWithTimeout2(); - roboclaw_.updateCrc(crc, datum); - result = datum << 8; - datum = roboclaw_.readByteWithTimeout2(); - roboclaw_.updateCrc(crc, datum); - result |= datum; - - uint16_t responseCrc = 0; - datum = roboclaw_.readByteWithTimeout2(); - responseCrc = datum << 8; - datum = roboclaw_.readByteWithTimeout2(); - responseCrc |= datum; - if (responseCrc != crc) { - RCUTILS_LOG_ERROR( - "[RoboClaw::CmdReadTemperature] invalid CRC expected: 0x%2X, " - "got: 0x%2X", - crc, responseCrc); - result = 0.0; - } - - temperature_ = result / 10.0; - } catch (...) { - RCUTILS_LOG_ERROR( - "[RoboClaw::CmdReadTemperature] Uncaught exception !!!"); - } - - roboclaw_.appendToReadLog(", RESULT: %f", temperature_); - } - - float &temperature_; - }; - - class CmdDoBufferedM1M2DriveSpeedAccelDistance : public Cmd { - public: - CmdDoBufferedM1M2DriveSpeedAccelDistance( - RoboClaw &roboclaw, uint32_t accel_quad_pulses_per_second, - int32_t m1_speed_quad_pulses_per_second, - uint32_t m1_max_distance_quad_pulses, - int32_t m2_speed_quad_pulses_per_second, - uint32_t m2_max_distance_quad_pulses) - : Cmd(roboclaw, "DoBufferedDriveSpeedAccelDistance", kNone), - accel_quad_pulses_per_second_(accel_quad_pulses_per_second), - m1_speed_quad_pulses_per_second_(m1_speed_quad_pulses_per_second), - m1_max_distance_quad_pulses_(m1_max_distance_quad_pulses), - m2_speed_quad_pulses_per_second_(m2_speed_quad_pulses_per_second), - m2_max_distance_quad_pulses_(m2_max_distance_quad_pulses) {} - void send() override { - try { - roboclaw_.appendToWriteLog( - "BufferedM1M2WithSignedSpeedAccelDist: accel: %d, m1Speed: %d, " - "m1Distance: %d, m2Speed: %d, m2Distance: %d, WROTE: ", - accel_quad_pulses_per_second_, m1_speed_quad_pulses_per_second_, - m1_max_distance_quad_pulses_, m2_speed_quad_pulses_per_second_, - m2_max_distance_quad_pulses_); - roboclaw_.writeN2(true, 23, roboclaw_.portAddress_, - kMIXEDSPEEDACCELDIST, - SetDWORDval(accel_quad_pulses_per_second_), - SetDWORDval(m1_speed_quad_pulses_per_second_), - SetDWORDval(m1_max_distance_quad_pulses_), - SetDWORDval(m2_speed_quad_pulses_per_second_), - SetDWORDval(m2_max_distance_quad_pulses_), - 1 /* Cancel any previous command. */ - ); - } catch (...) { - RCUTILS_LOG_ERROR( - "[RoboClaw::CmdDoBufferedM1M2DriveSpeedAccelDistance] Uncaught " - "exception !!!"); - } - } - - uint32_t accel_quad_pulses_per_second_; - int32_t m1_speed_quad_pulses_per_second_; - uint32_t m1_max_distance_quad_pulses_; - int32_t m2_speed_quad_pulses_per_second_; - uint32_t m2_max_distance_quad_pulses_; - }; - - friend class Cmd; // Make Cmd a friend class of RoboClaw + friend class Cmd; + friend class CmdDoBufferedM1M2DriveSpeedAccelDistance; + friend class CmdReadEncoderSpeed; + friend class CmdReadEncoder; + friend class CmdReadFirmwareVersion; + friend class CmdReadLogicBatteryVoltage; + friend class CmdReadMainBatteryVoltage; + friend class CmdReadMotorCurrents; + friend class CmdReadMotorVelocityPIDQ; + friend class CmdReadStatus; + friend class CmdReadTemperature; + friend class CmdSetEncoderValue; + friend class CmdSetPid; protected: DebugLog debug_log_; diff --git a/include/roboclaw_cmd.h b/include/roboclaw_cmd.h new file mode 100644 index 0000000..45d8158 --- /dev/null +++ b/include/roboclaw_cmd.h @@ -0,0 +1,47 @@ +#pragma once + +#include "roboclaw.h" + +class Cmd { + public: + void execute() { + for (int retry = 0; retry < 3 /*### maxCommandRetries_*/; retry++) { + try { + std::lock_guard lock( + RoboClaw::buffered_command_mutex_); // Lock the mutex + send(); + roboclaw_.debug_log_.showLog(); + return; + } catch (RoboClaw::TRoboClawException *e) { + roboclaw_.debug_log_.showLog(); + RCUTILS_LOG_ERROR( + "[RoboClaw::Cmd::execute] Exception: %s, retry number: %d", + e->what(), retry); + } catch (...) { + roboclaw_.debug_log_.showLog(); + RCUTILS_LOG_ERROR("[RoboClaw::Cmd::execute] Uncaught exception !!!"); + } + } + + roboclaw_.debug_log_.showLog(); + RCUTILS_LOG_ERROR("[RoboClaw::Cmd::execute] RETRY COUNT EXCEEDED"); + throw new RoboClaw::TRoboClawException( + "[RoboClaw::Cmd::execute] RETRY COUNT EXCEEDED"); + } + + virtual void send() = 0; // Declare send as a pure virtual function + + protected: + Cmd(RoboClaw &roboclaw, const char *name, const RoboClaw::kMotor motor) + : motor_(motor), roboclaw_(roboclaw) { + strncpy(name_, name, sizeof(name_)); + name_[sizeof(name_) - 1] = '\0'; // Ensure null-termination + } + + RoboClaw::kMotor motor_; + RoboClaw &roboclaw_; + char name_[32]; + + private: + Cmd() = delete; // Disallow default constructor +}; diff --git a/include/roboclaw_cmd_do_buffered_m1m2_drive_speed_accel_distance.h b/include/roboclaw_cmd_do_buffered_m1m2_drive_speed_accel_distance.h new file mode 100644 index 0000000..bcbfaf3 --- /dev/null +++ b/include/roboclaw_cmd_do_buffered_m1m2_drive_speed_accel_distance.h @@ -0,0 +1,48 @@ +#pragma once + +#include "roboclaw_cmd.h" + +class CmdDoBufferedM1M2DriveSpeedAccelDistance : public Cmd { + public: + CmdDoBufferedM1M2DriveSpeedAccelDistance( + RoboClaw &roboclaw, uint32_t accel_quad_pulses_per_second, + int32_t m1_speed_quad_pulses_per_second, + uint32_t m1_max_distance_quad_pulses, + int32_t m2_speed_quad_pulses_per_second, + uint32_t m2_max_distance_quad_pulses) + : Cmd(roboclaw, "DoBufferedDriveSpeedAccelDistance", RoboClaw::kNone), + accel_quad_pulses_per_second_(accel_quad_pulses_per_second), + m1_speed_quad_pulses_per_second_(m1_speed_quad_pulses_per_second), + m1_max_distance_quad_pulses_(m1_max_distance_quad_pulses), + m2_speed_quad_pulses_per_second_(m2_speed_quad_pulses_per_second), + m2_max_distance_quad_pulses_(m2_max_distance_quad_pulses) {} + void send() override { + try { + roboclaw_.appendToWriteLog( + "BufferedM1M2WithSignedSpeedAccelDist: accel: %d, m1Speed: %d, " + "m1Distance: %d, m2Speed: %d, m2Distance: %d, WROTE: ", + accel_quad_pulses_per_second_, m1_speed_quad_pulses_per_second_, + m1_max_distance_quad_pulses_, m2_speed_quad_pulses_per_second_, + m2_max_distance_quad_pulses_); + roboclaw_.writeN2(true, 23, roboclaw_.portAddress_, + RoboClaw::kMIXEDSPEEDACCELDIST, + SetDWORDval(accel_quad_pulses_per_second_), + SetDWORDval(m1_speed_quad_pulses_per_second_), + SetDWORDval(m1_max_distance_quad_pulses_), + SetDWORDval(m2_speed_quad_pulses_per_second_), + SetDWORDval(m2_max_distance_quad_pulses_), + 1 /* Cancel any previous command. */ + ); + } catch (...) { + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdDoBufferedM1M2DriveSpeedAccelDistance] Uncaught " + "exception !!!"); + } + } + + uint32_t accel_quad_pulses_per_second_; + int32_t m1_speed_quad_pulses_per_second_; + uint32_t m1_max_distance_quad_pulses_; + int32_t m2_speed_quad_pulses_per_second_; + uint32_t m2_max_distance_quad_pulses_; +}; diff --git a/include/roboclaw_cmd_read_encoder.h b/include/roboclaw_cmd_read_encoder.h new file mode 100644 index 0000000..c6b32ae --- /dev/null +++ b/include/roboclaw_cmd_read_encoder.h @@ -0,0 +1,69 @@ +#pragma once + +#include "roboclaw_cmd.h" + +class CmdReadEncoder : public Cmd { + public: + CmdReadEncoder(RoboClaw &roboclaw, RoboClaw::kMotor motor, + RoboClaw::EncodeResult &encoder) + : Cmd(roboclaw, "ReadEncoder", motor), encoder_(encoder) {} + void send() override { + try { + roboclaw_.appendToWriteLog("ReadEncoder: encoder: %d (%s), WROTE: ", + motor_, motor_ == RoboClaw::kM1 ? "M1" : "M2"); + + uint16_t crc = 0; + roboclaw_.updateCrc(crc, roboclaw_.portAddress_); + roboclaw_.updateCrc(crc, motor_ == RoboClaw::kM1 + ? RoboClaw::kGETM1ENC + : RoboClaw::RoboClaw::kGETM2ENC); + + roboclaw_.writeN2( + false, 2, roboclaw_.portAddress_, + motor_ == RoboClaw::kM1 ? RoboClaw::kGETM1ENC : RoboClaw::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 RoboClaw::TRoboClawException( + "[RoboClaw::CmdReadEncoder] INVALID CRC"); + } + + roboclaw_.appendToReadLog(", RESULT value: %d, status: %d", + encoder_.value, encoder_.status); + return; + } catch (...) { + RCUTILS_LOG_ERROR("[RoboClaw::CmdReadEncoder] Uncaught exception !!!"); + } + } + + RoboClaw::EncodeResult &encoder_; +}; diff --git a/include/roboclaw_cmd_read_encoder_speed.h b/include/roboclaw_cmd_read_encoder_speed.h new file mode 100644 index 0000000..7f70916 --- /dev/null +++ b/include/roboclaw_cmd_read_encoder_speed.h @@ -0,0 +1,27 @@ +#pragma once + +#include "roboclaw_cmd.h" + +class CmdReadEncoderSpeed : public Cmd { + public: + CmdReadEncoderSpeed(RoboClaw &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_ == RoboClaw::kM1 ? "M1" : "M2"); + speed_ = roboclaw_.getVelocityResult(motor_ == RoboClaw::kM1 + ? RoboClaw::kGETM1SPEED + : RoboClaw::kGETM2SPEED); + roboclaw_.appendToReadLog(", RESULT: %d", speed_); + return; + } catch (...) { + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadEncoderSpeed] Uncaught exception " + "!!!"); + } + } + + int32_t &speed_; +}; diff --git a/include/roboclaw_cmd_read_firmware_version.h b/include/roboclaw_cmd_read_firmware_version.h new file mode 100644 index 0000000..e6e6347 --- /dev/null +++ b/include/roboclaw_cmd_read_firmware_version.h @@ -0,0 +1,65 @@ +#pragma once + +#include "roboclaw_cmd.h" + +class CmdReadFirmwareVersion : public Cmd { + public: + CmdReadFirmwareVersion(RoboClaw &roboclaw, std::string &version) + : Cmd(roboclaw, "ReadFirmwareVersion", RoboClaw::kNone), + version_(version) {} + + void send() override { + roboclaw_.appendToWriteLog("ReadFirmwareVersion: WROTE: "); + roboclaw_.writeN2(false, 2, roboclaw_.portAddress_, RoboClaw::kGETVERSION); + + try { + uint16_t crc = 0; + uint8_t i; + uint8_t datum; + std::stringstream version; + + roboclaw_.updateCrc(crc, roboclaw_.portAddress_); + roboclaw_.updateCrc(crc, RoboClaw::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_.appendToReadLog(", RESULT: '%s'", version_.c_str()); + return; + } else { + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadFirmwareVersion] invalid " + "CRC expected: 0x%02X, " + "got: 0x%02X", + crc, responseCrc); + throw new RoboClaw::TRoboClawException( + "[RoboClaw::getVersion] Invalid CRC, expected 0x%02X, " + "got 0x%02X", + crc, responseCrc); + } + } else { + version << (char)datum; + } + } + + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadFirmwareVersion] unexpected long string"); + throw new RoboClaw::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_; +}; diff --git a/include/roboclaw_cmd_read_logic_battery_voltage.h b/include/roboclaw_cmd_read_logic_battery_voltage.h new file mode 100644 index 0000000..54818cf --- /dev/null +++ b/include/roboclaw_cmd_read_logic_battery_voltage.h @@ -0,0 +1,25 @@ +#pragma once + +#include "roboclaw_cmd.h" + +class CmdReadLogicBatteryVoltage : public Cmd { + public: + CmdReadLogicBatteryVoltage(RoboClaw &roboclaw, float &voltage) + : Cmd(roboclaw, "ReadLogicBatteryVoltage", RoboClaw::kNone), + voltage_(voltage) {} + void send() override { + try { + roboclaw_.appendToWriteLog("ReadLogicBatteryVoltage: WROTE: "); + float result = + ((float)roboclaw_.get2ByteCommandResult2(RoboClaw::kGETLBATT)) / 10.0; + voltage_ = result; + roboclaw_.appendToReadLog(", RESULT: %f", result); + return; + } catch (...) { + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadLogicBatteryVoltage] Uncaught exception !!!"); + } + } + + float &voltage_; +}; diff --git a/include/roboclaw_cmd_read_main_battery_voltage.h b/include/roboclaw_cmd_read_main_battery_voltage.h new file mode 100644 index 0000000..b4a2a72 --- /dev/null +++ b/include/roboclaw_cmd_read_main_battery_voltage.h @@ -0,0 +1,25 @@ +#pragma once + +#include "roboclaw_cmd.h" + +class CmdReadMainBatteryVoltage : public Cmd { + public: + CmdReadMainBatteryVoltage(RoboClaw &roboclaw, float &voltage) + : Cmd(roboclaw, "ReadLMainBatteryVoltage", RoboClaw::kNone), + voltage_(voltage) {} + void send() override { + try { + roboclaw_.appendToWriteLog("ReadLMainBatteryVoltage: WROTE: "); + float result = + ((float)roboclaw_.get2ByteCommandResult2(RoboClaw::kGETMBATT)) / 10.0; + voltage_ = result; + roboclaw_.appendToReadLog(", RESULT: %f", result); + return; + } catch (...) { + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadMainBatteryVoltage] Uncaught exception !!!"); + } + } + + float &voltage_; +}; diff --git a/include/roboclaw_cmd_read_motor_currents.h b/include/roboclaw_cmd_read_motor_currents.h new file mode 100644 index 0000000..b53199e --- /dev/null +++ b/include/roboclaw_cmd_read_motor_currents.h @@ -0,0 +1,51 @@ +#pragma once + +#include "roboclaw_cmd.h" + +class CmdReadMotorCurrents : public Cmd { + public: + CmdReadMotorCurrents(RoboClaw &roboclaw, + RoboClaw::TMotorCurrents &motorCurrents) + : Cmd(roboclaw, "ReadMotorCurrents", RoboClaw::kNone), + motorCurrents_(motorCurrents) {} + void send() override { + try { + roboclaw_.appendToWriteLog("ReadMotorCurrents: WROTE: "); + + unsigned long currentPair = + roboclaw_.getUlongCommandResult2(RoboClaw::kGETCURRENTS); + motorCurrents_.m1Current = ((int16_t)(currentPair >> 16)) * 0.010; + motorCurrents_.m2Current = ((int16_t)(currentPair & 0xFFFF)) * 0.010; + roboclaw_.appendToReadLog(", RESULT m1 current: %3.4f, m2 current: %3.4f", + motorCurrents_.m1Current, + motorCurrents_.m2Current); + + if (motorCurrents_.m1Current > roboclaw_.maxM1Current_) { + roboclaw_.motorAlarms_ |= RoboClaw::kM1_OVER_CURRENT_ALARM; + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadMotorCurrents] Motor 1 over current. Max " + "allowed: %6.3f, found: %6.3f", + roboclaw_.maxM1Current_, motorCurrents_.m1Current); + roboclaw_.stop(); + } else { + roboclaw_.motorAlarms_ &= ~RoboClaw::kM1_OVER_CURRENT_ALARM; + } + + if (motorCurrents_.m2Current > roboclaw_.maxM2Current_) { + roboclaw_.motorAlarms_ |= RoboClaw::kM2_OVER_CURRENT_ALARM; + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadMotorCurrents] Motor 2 over current. Max " + "allowed: %6.3f, found: %6.3f", + roboclaw_.maxM2Current_, motorCurrents_.m2Current); + roboclaw_.stop(); + } else { + roboclaw_.motorAlarms_ &= ~RoboClaw::kM2_OVER_CURRENT_ALARM; + } + } catch (...) { + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadMotorCurrents] Uncaught exception !!!"); + } + } + + RoboClaw::TMotorCurrents &motorCurrents_; +}; diff --git a/include/roboclaw_cmd_read_motor_velocity_pidq.h b/include/roboclaw_cmd_read_motor_velocity_pidq.h new file mode 100644 index 0000000..4f2bdc8 --- /dev/null +++ b/include/roboclaw_cmd_read_motor_velocity_pidq.h @@ -0,0 +1,54 @@ +#pragma once + +#include "roboclaw_cmd.h" + +class CmdReadMotorVelocityPIDQ : public Cmd { + public: + CmdReadMotorVelocityPIDQ(RoboClaw &roboclaw, RoboClaw::kMotor motor, + RoboClaw::TPIDQ &pidq) + : Cmd(roboclaw, "ReadMotorVelocityPIDQ", motor), pidq_(pidq) {} + void send() override { + try { + roboclaw_.appendToWriteLog( + "ReadMotorVelocityPIDQ: motor: %d (%s), WROTE: ", motor_, + motor_ == RoboClaw::kM1 ? "M1" : "M2"); + RoboClaw::TPIDQ result; + uint16_t crc = 0; + roboclaw_.updateCrc(crc, roboclaw_.portAddress_); + roboclaw_.updateCrc(crc, motor_ == RoboClaw::kM1 ? RoboClaw::kGETM1PID + : RoboClaw::kGETM2PID); + + roboclaw_.writeN2( + false, 2, roboclaw_.portAddress_, + motor_ == RoboClaw::kM1 ? RoboClaw::kGETM1PID : RoboClaw::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); + 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 !!!"); + } + } + + RoboClaw::TPIDQ &pidq_; +}; \ No newline at end of file diff --git a/include/roboclaw_cmd_read_status.h b/include/roboclaw_cmd_read_status.h new file mode 100644 index 0000000..3a78fb5 --- /dev/null +++ b/include/roboclaw_cmd_read_status.h @@ -0,0 +1,38 @@ +#pragma once + +#include "roboclaw_cmd.h" + +class CmdReadStatus : public Cmd { + public: + CmdReadStatus(RoboClaw &roboclaw, uint16_t &status) + : Cmd(roboclaw, "ReadStatus", RoboClaw::kNone), status_(status) {} + void send() override { + try { + uint16_t crc = 0; + roboclaw_.updateCrc(crc, roboclaw_.portAddress_); + roboclaw_.updateCrc(crc, RoboClaw::kGETERROR); + roboclaw_.appendToWriteLog("ReadStatus: WROTE: "); + roboclaw_.writeN2(false, 2, roboclaw_.portAddress_, RoboClaw::kGETERROR); + status_ = (unsigned short)roboclaw_.getULongCont2(crc); + uint16_t responseCrc = 0; + uint16_t datum = roboclaw_.readByteWithTimeout2(); + responseCrc = datum << 8; + datum = roboclaw_.readByteWithTimeout2(); + responseCrc |= datum; + if (responseCrc == crc) { + roboclaw_.appendToReadLog(", RESULT: %04X", status_); + return; + } else { + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadStatus] invalid CRC expected: 0x%02X, " + "got: " + "0x%02X", + crc, responseCrc); + } + } catch (...) { + RCUTILS_LOG_ERROR("[RoboClaw::CmdReadStatus] Uncaught exception !!!"); + } + }; + + uint16_t &status_; +}; diff --git a/include/roboclaw_cmd_read_temperature.h b/include/roboclaw_cmd_read_temperature.h new file mode 100644 index 0000000..08d5ebd --- /dev/null +++ b/include/roboclaw_cmd_read_temperature.h @@ -0,0 +1,49 @@ +#pragma once + +#include "roboclaw_cmd.h" + +class CmdReadTemperature : public Cmd { + public: + CmdReadTemperature(RoboClaw &roboclaw, float &temperature) + : Cmd(roboclaw, "ReadTemperature", RoboClaw::kNone), + temperature_(temperature) {} + void send() override { + try { + roboclaw_.appendToWriteLog("ReadTemperature: WROTE: "); + uint16_t crc = 0; + roboclaw_.updateCrc(crc, roboclaw_.portAddress_); + roboclaw_.updateCrc(crc, RoboClaw::kGETTEMPERATURE); + roboclaw_.writeN2(false, 2, roboclaw_.portAddress_, + RoboClaw::kGETTEMPERATURE); + uint16_t result = 0; + uint8_t datum = roboclaw_.readByteWithTimeout2(); + roboclaw_.updateCrc(crc, datum); + result = datum << 8; + datum = roboclaw_.readByteWithTimeout2(); + roboclaw_.updateCrc(crc, datum); + result |= datum; + + uint16_t responseCrc = 0; + datum = roboclaw_.readByteWithTimeout2(); + responseCrc = datum << 8; + datum = roboclaw_.readByteWithTimeout2(); + responseCrc |= datum; + if (responseCrc != crc) { + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadTemperature] invalid CRC expected: 0x%2X, " + "got: 0x%2X", + crc, responseCrc); + result = 0.0; + } + + temperature_ = result / 10.0; + } catch (...) { + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdReadTemperature] Uncaught exception !!!"); + } + + roboclaw_.appendToReadLog(", RESULT: %f", temperature_); + } + + float &temperature_; +}; diff --git a/include/roboclaw_cmd_set_encoder_value.h b/include/roboclaw_cmd_set_encoder_value.h new file mode 100644 index 0000000..dc72263 --- /dev/null +++ b/include/roboclaw_cmd_set_encoder_value.h @@ -0,0 +1,27 @@ +#pragma once + +#include "roboclaw_cmd.h" + +class CmdSetEncoderValue : public Cmd { + public: + CmdSetEncoderValue(RoboClaw &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_, + RoboClaw::motorNames_[motor_], value_); + try { + roboclaw_.writeN2(true, 6, roboclaw_.portAddress_, + motor_ == RoboClaw::kM1 ? RoboClaw::kSETM1ENCODER + : RoboClaw::kSETM2ENCODER, + SetDWORDval(value_)); + return; + } catch (...) { + RCUTILS_LOG_ERROR( + "[RoboClaw::CmdSetEncoderValue] Uncaught exception !!!"); + } + } + + long value_; +}; diff --git a/include/roboclaw_cmd_set_pid.h b/include/roboclaw_cmd_set_pid.h new file mode 100644 index 0000000..cf1e9f7 --- /dev/null +++ b/include/roboclaw_cmd_set_pid.h @@ -0,0 +1,29 @@ +#pragma once + +#include "roboclaw_cmd.h" + +class CmdSetPid : public Cmd { + public: + CmdSetPid(RoboClaw &roboclaw, RoboClaw::kMotor motor, float p, float i, + float d, uint32_t qpps) + : Cmd(roboclaw, "SetPid", motor), p_(p), i_(i), d_(d), qpps_(qpps) {} + + void send() override { + roboclaw_.appendToWriteLog( + "SetPid: motor: %d (%s) p: %f, i: %f, d: %f, " + "qpps: %d, WROTE: ", + motor_, RoboClaw::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); + roboclaw_.writeN2( + true, 18, roboclaw_.portAddress_, + motor_ == RoboClaw::kM1 ? RoboClaw::kSETM1PID : RoboClaw::kSETM2PID, + SetDWORDval(kd), SetDWORDval(kp), SetDWORDval(ki), SetDWORDval(qpps_)); + } + + float p_; + float i_; + float d_; + uint32_t qpps_; +}; diff --git a/src/roboclaw.cpp b/src/roboclaw.cpp index 4328ed6..13b8f33 100755 --- a/src/roboclaw.cpp +++ b/src/roboclaw.cpp @@ -15,6 +15,18 @@ #include #include +#include "roboclaw_cmd_do_buffered_m1m2_drive_speed_accel_distance.h" +#include "roboclaw_cmd_read_encoder.h" +#include "roboclaw_cmd_read_encoder_speed.h" +#include "roboclaw_cmd_read_firmware_version.h" +#include "roboclaw_cmd_read_logic_battery_voltage.h" +#include "roboclaw_cmd_read_main_battery_voltage.h" +#include "roboclaw_cmd_read_motor_currents.h" +#include "roboclaw_cmd_read_motor_velocity_pidq.h" +#include "roboclaw_cmd_read_status.h" +#include "roboclaw_cmd_read_temperature.h" +#include "roboclaw_cmd_set_encoder_value.h" +#include "roboclaw_cmd_set_pid.h" #include "ros2_roboclaw_driver/srv/reset_encoders.h" const char *RoboClaw::motorNames_[] = {"M1", "M2", "NONE"};