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

Broke out all Cmd class files into their own files for better organization and readability.

Browse Source
This commit is contained in:
Michael Wimble
2025-03-22 13:01:53 -07:00
parent f2cf5c6aee
commit b9c51591c0
16 changed files with 593 additions and 500 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
config/motor_driver.yaml
View File

@@ -9,7 +9,7 @@ motor_driver_node:
accel_quad_pulses_per_second: 1000
# The device name to be opened.
device_name: "/dev/ttyAMA1"
device_name: "/dev/ttyACM2"
baud_rate: 38400
# The assigned port for the device (as configured on the RoboClaw).

525
include/roboclaw.h
View File

@@ -12,6 +12,19 @@
#include <string>
#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<std::mutex> 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_;

47
include/roboclaw_cmd.h Normal file
View File

@@ -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<std::mutex> 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
};

48
include/roboclaw_cmd_do_buffered_m1m2_drive_speed_accel_distance.h Normal file
View File

@@ -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_;
};

69
include/roboclaw_cmd_read_encoder.h Normal file
View File

@@ -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_;
};

27
include/roboclaw_cmd_read_encoder_speed.h Normal file
View File

@@ -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_;
};

65
include/roboclaw_cmd_read_firmware_version.h Normal file
View File

@@ -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_;
};

25
include/roboclaw_cmd_read_logic_battery_voltage.h Normal file
View File

@@ -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_;
};

25
include/roboclaw_cmd_read_main_battery_voltage.h Normal file
View File

@@ -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_;
};

51
include/roboclaw_cmd_read_motor_currents.h Normal file
View File

@@ -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_;
};

54
include/roboclaw_cmd_read_motor_velocity_pidq.h Normal file
View File

@@ -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_;
};

38
include/roboclaw_cmd_read_status.h Normal file
View File

@@ -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_;
};

49
include/roboclaw_cmd_read_temperature.h Normal file
View File

@@ -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_;
};

27
include/roboclaw_cmd_set_encoder_value.h Normal file
View File

@@ -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_;
};

29
include/roboclaw_cmd_set_pid.h Normal file
View File

@@ -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_;
};

12
src/roboclaw.cpp
View File

@@ -15,6 +15,18 @@
#include <rclcpp/rclcpp.hpp>
#include <sstream>
#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"};