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

Snapshot

Browse Source
This commit is contained in:
Michael Wimble
2025-03-19 18:01:36 -07:00
parent df421a8b79
commit c0d86f1258
4 changed files with 357 additions and 560 deletions
Download Patch File Download Diff File Expand all files Collapse all files

229
include/roboclaw.h
View File

@@ -31,6 +31,8 @@
class RoboClaw { class RoboClaw {
public: public:
enum kMotor { kM1 = 0, kM2 = 1, kNone = 2 };
// Bit positions used to build alarms. // Bit positions used to build alarms.
enum { enum {
kM1_OVER_CURRENT = 0x01, // Motor 1 current sense is too high. kM1_OVER_CURRENT = 0x01, // Motor 1 current sense is too high.
@@ -112,7 +114,7 @@ public:
int32_t m2_quad_pulses_per_second, int32_t m2_quad_pulses_per_second,
uint32_t m2_max_distance); uint32_t m2_max_distance);
EncodeResult getEncoderCommandResult(WHICH_ENC command); // EncodeResult getEncoderCommandResult(WHICH_ENC command);
// Get RoboClaw error status bits. // Get RoboClaw error status bits.
uint16_t getErrorStatus(); uint16_t getErrorStatus();
@@ -127,9 +129,13 @@ public:
// Get the encoder value for motor 1. // Get the encoder value for motor 1.
int32_t getM1Encoder(); int32_t getM1Encoder();
int8_t getM1EncoderStatus();
// Get the encoder value for motor 2. // Get the encoder value for motor 2.
int32_t getM2Encoder(); int32_t getM2Encoder();
int8_t getM2EncoderStatus();
// Convenience structure to hold a pair of current values. // Convenience structure to hold a pair of current values.
typedef struct { typedef struct {
float m1Current; float m1Current;
@@ -141,7 +147,8 @@ public:
TMotorCurrents getMotorCurrents(); TMotorCurrents getMotorCurrents();
TPIDQ getPIDQ(WHICH_MOTOR whichMotor); TPIDQ getPIDQM1();
TPIDQ getPIDQM2();
float getTemperature(); float getTemperature();
@@ -157,7 +164,7 @@ public:
// Get singleton instance of class. // Get singleton instance of class.
static RoboClaw *singleton(); static RoboClaw *singleton();
static void readSensorGroup(); void readSensorGroup();
protected: protected:
// Write a stream of bytes to the device. // Write a stream of bytes to the device.
@@ -172,36 +179,25 @@ private:
// True => print byte values as they are read and written. // True => print byte values as they are read and written.
bool doLowLevelDebug_; bool doLowLevelDebug_;
EncodeResult cache_getEncoderCommandResult(WHICH_ENC command);
// Get RoboClaw error status bits. // Get RoboClaw error status bits.
uint16_t cache_getErrorStatus(); uint16_t cache_getErrorStatus();
// Get RoboClaw error status as a string. // Get RoboClaw error status as a string.
std::string cache_getErrorString(); std::string cache_getErrorString();
float cache_getLogicBatteryLevel();
// Get the encoder value for motor 1.
int32_t cache_getM1Encoder();
int32_t cache_getM1Speed(); int32_t cache_getM1Speed();
// Get the encoder value for motor 2.
int32_t cache_getM2Encoder();
int32_t cache_getM2Speed(); int32_t cache_getM2Speed();
float cache_getMainBatteryLevel(); float cache_getMainBatteryLevel();
TMotorCurrents cache_getMotorCurrents(); TMotorCurrents cache_getMotorCurrents();
TPIDQ cache_getPIDQ(WHICH_MOTOR whichMotor);
float cache_getTemperature(); float cache_getTemperature();
// Get velocity (speed) of a motor. // Get velocity (speed) of a motor.
int32_t cache_getVelocity(WHICH_VELOCITY whichVelocity); int32_t cache_getVelocityM1();
int32_t cache_getVelocityM2();
static void sensorReadThread(); static void sensorReadThread();
@@ -209,14 +205,12 @@ private:
uint16_t error_status; uint16_t error_status;
std::string error_string; std::string error_string;
float logic_battery_level; float logic_battery_level;
int32_t m1_encoder;
EncodeResult m1_encoder_command_result; EncodeResult m1_encoder_command_result;
TPIDQ m1_pidq; TPIDQ m1_pidq;
int32_t m1_velocity; int32_t m1_velocity;
int32_t m2_encoder;
EncodeResult m2_encoder_command_result; EncodeResult m2_encoder_command_result;
TPIDQ m2_pidq; TPIDQ m2_pidq;
int32_t m2_speed; int32_t m2_velocity;
float main_battery_level; float main_battery_level;
int motor_alarms; int motor_alarms;
TMotorCurrents motor_currents; TMotorCurrents motor_currents;
@@ -224,7 +218,7 @@ private:
std::chrono::system_clock::time_point last_sensor_read_time_; std::chrono::system_clock::time_point last_sensor_read_time_;
} SensorValueGroup; } SensorValueGroup;
static SensorValueGroup g_sensor_value_group_; SensorValueGroup g_sensor_value_group_;
typedef struct { typedef struct {
unsigned long p1; unsigned long p1;
@@ -322,10 +316,12 @@ private:
unsigned long getUlongCommandResult(uint8_t command); 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 get2ByteCommandResult(uint8_t command);
unsigned short get2ByteCommandResult2(uint8_t command);
// Open the RoboClaw USB port. // Open the RoboClaw USB port.
void openPort(); void openPort();
@@ -365,8 +361,6 @@ private:
char command_log_[256]; char command_log_[256];
char response_log_[256]; char response_log_[256];
enum kMotor { kM1 = 0, kM2 = 1, kNone = 2 };
class Cmd { class Cmd {
public: public:
void execute() { void execute() {
@@ -426,6 +420,9 @@ private:
if (written > 0) { if (written > 0) {
next_write_log_index_ += written; next_write_log_index_ += written;
} }
if ((next_write_log_index_ > 0) && (write_log_[0] == '8')) {
RCUTILS_LOG_INFO("Whoops!");
}
} }
void showLog() { void showLog() {
@@ -498,6 +495,7 @@ private:
responseCrc |= datum; responseCrc |= datum;
if (responseCrc == crc) { if (responseCrc == crc) {
version_ = version.str(); version_ = version.str();
roboclaw_.appendToReadLog(", RESULT: '%s'", version_.c_str());
roboclaw_.debug_log_.showLog(); roboclaw_.debug_log_.showLog();
return; return;
} else { } else {
@@ -564,7 +562,7 @@ private:
roboclaw_.updateCrc(crc, kGETERROR); roboclaw_.updateCrc(crc, kGETERROR);
roboclaw_.appendToWriteLog("ReadStatus: WROTE: "); roboclaw_.appendToWriteLog("ReadStatus: WROTE: ");
roboclaw_.writeN2(false, 2, roboclaw_.portAddress_, kGETERROR); 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 responseCrc = 0;
uint16_t datum = roboclaw_.readByteWithTimeout2(); uint16_t datum = roboclaw_.readByteWithTimeout2();
responseCrc = datum << 8; responseCrc = datum << 8;
@@ -577,25 +575,200 @@ private:
return; return;
} else { } else {
RCUTILS_LOG_ERROR( RCUTILS_LOG_ERROR(
"[RoboClaw::cache_getErrorStatus] invalid CRC expected: 0x%02X, " "[RoboClaw::CmdReadStatus] invalid CRC expected: 0x%02X, "
"got: " "got: "
"0x%02X", "0x%02X",
crc, responseCrc); crc, responseCrc);
} }
} catch (...) { } catch (...) {
RCUTILS_LOG_ERROR( RCUTILS_LOG_ERROR("[RoboClaw::CmdReadStatus] Uncaught exception !!!");
"[RoboClaw::cache_getErrorStatus] Uncaught exception !!!");
} }
}; };
unsigned short &status_; 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 friend class Cmd; // Make Cmd a friend class of RoboClaw
protected: protected:
DebugLog debug_log_; DebugLog debug_log_;
static const char *commandNames_[];
static const char *motorNames_[]; static const char *motorNames_[];
}; };

6
launch/ros2_roboclaw_driver.launch.py
View File

@@ -6,6 +6,7 @@ from launch import LaunchDescription
import launch_ros.actions import launch_ros.actions
from launch_ros.actions import Node from launch_ros.actions import Node
def generate_launch_description(): def generate_launch_description():
# The following 'my_package_name' should be changed to the # The following 'my_package_name' should be changed to the
# package name in your robot workspace that will contain # package name in your robot workspace that will contain
@@ -37,7 +38,8 @@ def generate_launch_description():
# file for more information. # file for more information.
with open(configFilePath, 'r') as file: 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() ld = LaunchDescription()
@@ -47,7 +49,7 @@ def generate_launch_description():
executable='ros2_roboclaw_driver_node', executable='ros2_roboclaw_driver_node',
package='ros2_roboclaw_driver', package='ros2_roboclaw_driver',
parameters=[configParams], parameters=[configParams],
#prefix=['xterm -e gdb -ex run --args'], # prefix=['xterm -e gdb -ex run --args'],
respawn=True, respawn=True,
output='screen') output='screen')
ld.add_action(motor_driver_node) ld.add_action(motor_driver_node)

86
src/motor_driver_node.cpp
View File

@@ -2,82 +2,80 @@
#include <string> #include <string>
#include "motor_driver.h" #include "motor_driver.h"
#include "ros2_roboclaw_driver/msg/robo_claw_status.hpp"
#include "roboclaw.h" #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::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 &motorDriver = MotorDriver::singleton();
motorDriver.onInit(node); motorDriver.onInit(node);
auto qos = rclcpp::QoS( auto qos = rclcpp::QoS(
rclcpp::QoSInitialization( rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 10));
RMW_QOS_POLICY_HISTORY_KEEP_LAST,
10));
qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
qos.avoid_ros_namespace_conventions(false); qos.avoid_ros_namespace_conventions(false);
std::string statusTopicName; std::string statusTopicName;
node->declare_parameter<std::string>("roboclaw_status_topic", "roboclaw_status"); node->declare_parameter<std::string>("roboclaw_status_topic",
"roboclaw_status");
node->get_parameter("roboclaw_status_topic", statusTopicName); 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<ros2_roboclaw_driver::msg::RoboClawStatus>::SharedPtr statusPublisher = node->create_publisher<ros2_roboclaw_driver::msg::RoboClawStatus>(statusTopicName, qos); rclcpp::Publisher<ros2_roboclaw_driver::msg::RoboClawStatus>::SharedPtr
statusPublisher =
node->create_publisher<ros2_roboclaw_driver::msg::RoboClawStatus>(
statusTopicName, qos);
ros2_roboclaw_driver::msg::RoboClawStatus roboClawStatus; ros2_roboclaw_driver::msg::RoboClawStatus roboClawStatus;
rclcpp::WallRate loop_rate(20); rclcpp::WallRate loop_rate(20);
while (rclcpp::ok()) while (rclcpp::ok()) {
{ try {
try roboClawStatus.logic_battery_voltage =
{ RoboClaw::singleton()->getLogicBatteryLevel();
roboClawStatus.logic_battery_voltage = RoboClaw::singleton()->getLogicBatteryLevel(); roboClawStatus.main_battery_voltage =
roboClawStatus.main_battery_voltage = RoboClaw::singleton()->getMainBatteryLevel(); RoboClaw::singleton()->getMainBatteryLevel();
RoboClaw::TMotorCurrents motorCurrents = RoboClaw::singleton()->getMotorCurrents(); RoboClaw::TMotorCurrents motorCurrents =
RoboClaw::singleton()->getMotorCurrents();
roboClawStatus.m1_motor_current = motorCurrents.m1Current; roboClawStatus.m1_motor_current = motorCurrents.m1Current;
roboClawStatus.m2_motor_current = motorCurrents.m2Current; roboClawStatus.m2_motor_current = motorCurrents.m2Current;
RoboClaw::TPIDQ pidq = RoboClaw::singleton()->getPIDQ(RoboClaw::kGETM1PID); RoboClaw::TPIDQ pidq = RoboClaw::singleton()->getPIDQM1();
roboClawStatus.m1_p = pidq.p / 65536.0;
roboClawStatus.m1_i = pidq.i / 65536.0; roboClawStatus.m1_p = pidq.p;
roboClawStatus.m1_d = pidq.d / 65536.0; roboClawStatus.m1_i = pidq.i;
roboClawStatus.m1_d = pidq.d;
roboClawStatus.m1_qpps = pidq.qpps; roboClawStatus.m1_qpps = pidq.qpps;
pidq = RoboClaw::singleton()->getPIDQ(RoboClaw::kGETM2PID); pidq = RoboClaw::singleton()->getPIDQM2();
roboClawStatus.m2_p = pidq.p / 65536.0; roboClawStatus.m2_p = pidq.p;
roboClawStatus.m2_i = pidq.i / 65536.0; roboClawStatus.m2_i = pidq.i;
roboClawStatus.m2_d = pidq.d / 65536.0; roboClawStatus.m2_d = pidq.d;
roboClawStatus.m2_qpps = pidq.qpps; roboClawStatus.m2_qpps = pidq.qpps;
roboClawStatus.temperature = RoboClaw::singleton()->getTemperature(); roboClawStatus.temperature = RoboClaw::singleton()->getTemperature();
{ roboClawStatus.m1_encoder_value = RoboClaw::singleton()->getM1Encoder();
RoboClaw::EncodeResult encoder = RoboClaw::singleton()->getEncoderCommandResult(RoboClaw::kGETM1ENC); roboClawStatus.m1_encoder_status =
roboClawStatus.m1_encoder_value = encoder.value; RoboClaw::singleton()->getM1EncoderStatus();
roboClawStatus.m1_encoder_status = encoder.status; roboClawStatus.m2_encoder_value = RoboClaw::singleton()->getM2Encoder();
} roboClawStatus.m2_encoder_status =
RoboClaw::singleton()->getM2EncoderStatus();
{ roboClawStatus.m1_current_speed =
RoboClaw::EncodeResult encoder = RoboClaw::singleton()->getEncoderCommandResult(RoboClaw::kGETM2ENC); RoboClaw::singleton()->getVelocity(RoboClaw::kGETM1SPEED);
roboClawStatus.m2_encoder_value = encoder.value; roboClawStatus.m2_current_speed =
roboClawStatus.m2_encoder_status = encoder.status; 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(); roboClawStatus.error_string = RoboClaw::singleton()->getErrorString();
statusPublisher->publish(roboClawStatus); statusPublisher->publish(roboClawStatus);
} } catch (RoboClaw::TRoboClawException *e) {
catch (RoboClaw::TRoboClawException *e)
{
RCUTILS_LOG_ERROR("[motor_driver_node] Exception: %s", e->what()); RCUTILS_LOG_ERROR("[motor_driver_node] Exception: %s", e->what());
} } catch (...) {
catch (...)
{
RCUTILS_LOG_ERROR("[motor_driver_node] Uncaught exception !!!"); RCUTILS_LOG_ERROR("[motor_driver_node] Uncaught exception !!!");
} }

596
src/roboclaw.cpp
View File

@@ -1,5 +1,6 @@
#include "roboclaw.h" #include "roboclaw.h"
#include <cstdint>
#include <fcntl.h> #include <fcntl.h>
#include <math.h> #include <math.h>
#include <poll.h> #include <poll.h>
@@ -17,262 +18,6 @@
#include "ros2_roboclaw_driver/srv/reset_encoders.h" #include "ros2_roboclaw_driver/srv/reset_encoders.h"
const char *RoboClaw::motorNames_[] = {"M1", "M2", "NONE"}; 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, RoboClaw::RoboClaw(const TPIDQ m1Pid, const TPIDQ m2Pid, float m1MaxCurrent,
float m2MaxCurrent, std::string device_name, 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() { uint16_t RoboClaw::getErrorStatus() {
return g_sensor_value_group_.error_status; return g_sensor_value_group_.error_status;
} }
@@ -477,54 +169,6 @@ float RoboClaw::getLogicBatteryLevel() {
return g_sensor_value_group_.logic_battery_level; 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() { float RoboClaw::getMainBatteryLevel() {
return g_sensor_value_group_.main_battery_level; 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() { RoboClaw::TMotorCurrents RoboClaw::getMotorCurrents() {
return g_sensor_value_group_.motor_currents; return g_sensor_value_group_.motor_currents;
} }
@@ -636,55 +314,18 @@ RoboClaw::TMotorCurrents RoboClaw::cache_getMotorCurrents() {
"[RoboClaw::cache_getMotorCurrents] RETRY COUNT EXCEEDED"); "[RoboClaw::cache_getMotorCurrents] RETRY COUNT EXCEEDED");
} }
RoboClaw::TPIDQ RoboClaw::getPIDQ(WHICH_MOTOR whichMotor) { RoboClaw::TPIDQ RoboClaw::getPIDQM1() {
if (whichMotor == kGETM1PID) { TPIDQ result;
return g_sensor_value_group_.m1_pidq; CmdReadMotorVelocityPIDQ cmd(*this, kM1, result);
} else { cmd.execute();
return g_sensor_value_group_.m2_pidq; return result;
}
} }
RoboClaw::TPIDQ RoboClaw::cache_getPIDQ(WHICH_MOTOR whichMotor) { RoboClaw::TPIDQ RoboClaw::getPIDQM2() {
int retry; TPIDQ result;
CmdReadMotorVelocityPIDQ cmd(*this, kM2, result);
for (retry = 0; retry < maxCommandRetries_; retry++) { cmd.execute();
TPIDQ result; return 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");
} }
float RoboClaw::getTemperature() { return g_sensor_value_group_.temperature; } float RoboClaw::getTemperature() { return g_sensor_value_group_.temperature; }
@@ -771,18 +412,18 @@ unsigned long RoboClaw::getUlongCommandResult(uint8_t command) {
return 0; return 0;
} }
uint32_t RoboClaw::getULongCont(uint16_t &crc) { uint32_t RoboClaw::getULongCont2(uint16_t &crc) {
uint32_t result = 0; uint32_t result = 0;
uint8_t datum = readByteWithTimeout(); uint8_t datum = readByteWithTimeout2();
result |= datum << 24; result |= datum << 24;
updateCrc(crc, datum); updateCrc(crc, datum);
datum = readByteWithTimeout(); datum = readByteWithTimeout2();
result |= datum << 16; result |= datum << 16;
updateCrc(crc, datum); updateCrc(crc, datum);
datum = readByteWithTimeout(); datum = readByteWithTimeout2();
result |= datum << 8; result |= datum << 8;
updateCrc(crc, datum); updateCrc(crc, datum);
datum = readByteWithTimeout(); datum = readByteWithTimeout2();
result |= datum; result |= datum;
updateCrc(crc, datum); updateCrc(crc, datum);
return result; return result;
@@ -792,29 +433,22 @@ int32_t RoboClaw::getVelocity(WHICH_VELOCITY whichVelocity) {
if (whichVelocity == kGETM1SPEED) { if (whichVelocity == kGETM1SPEED) {
return g_sensor_value_group_.m1_velocity; return g_sensor_value_group_.m1_velocity;
} else { } else {
return g_sensor_value_group_.m2_speed; return g_sensor_value_group_.m2_velocity;
} }
} }
int32_t RoboClaw::cache_getVelocity(WHICH_VELOCITY whichVelocity) { int32_t RoboClaw::cache_getVelocityM1() {
int retry; int32_t result;
CmdReadEncoderSpeed cmd(*this, kM1, result);
cmd.execute();
return result;
}
for (retry = 0; retry < maxCommandRetries_; retry++) { int32_t RoboClaw::cache_getVelocityM2() {
try { int32_t result;
uint32_t result = getVelocityResult(whichVelocity); CmdReadEncoderSpeed cmd(*this, kM2, result);
return result; cmd.execute();
} catch (TRoboClawException *e) { return result;
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::getVelocityResult(uint8_t command) { int32_t RoboClaw::getVelocityResult(uint8_t command) {
@@ -822,33 +456,33 @@ int32_t RoboClaw::getVelocityResult(uint8_t command) {
updateCrc(crc, portAddress_); updateCrc(crc, portAddress_);
updateCrc(crc, command); updateCrc(crc, command);
writeN(false, 2, portAddress_, command); writeN2(false, 2, portAddress_, command);
int32_t result = 0; int32_t result = 0;
uint8_t datum = readByteWithTimeout(); uint8_t datum = readByteWithTimeout2();
result |= datum << 24; result |= datum << 24;
updateCrc(crc, datum); updateCrc(crc, datum);
datum = readByteWithTimeout(); datum = readByteWithTimeout2();
result |= datum << 16; result |= datum << 16;
updateCrc(crc, datum); updateCrc(crc, datum);
datum = readByteWithTimeout(); datum = readByteWithTimeout2();
result |= datum << 8; result |= datum << 8;
updateCrc(crc, datum); updateCrc(crc, datum);
datum = readByteWithTimeout(); datum = readByteWithTimeout2();
result |= datum; result |= datum;
updateCrc(crc, datum); updateCrc(crc, datum);
uint8_t direction = readByteWithTimeout(); uint8_t direction = readByteWithTimeout2();
updateCrc(crc, direction); updateCrc(crc, direction);
if (direction != 0) if (direction != 0)
result = -result; result = -result;
uint16_t responseCrc = 0; uint16_t responseCrc = 0;
datum = readByteWithTimeout(); datum = readByteWithTimeout2();
responseCrc = datum << 8; responseCrc = datum << 8;
datum = readByteWithTimeout(); datum = readByteWithTimeout2();
responseCrc |= datum; responseCrc |= datum;
if (responseCrc == crc) { if (responseCrc == crc) {
return result; return result;
@@ -862,28 +496,20 @@ int32_t RoboClaw::getVelocityResult(uint8_t command) {
return 0; 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() { int8_t RoboClaw::getM1EncoderStatus() {
int retry; return g_sensor_value_group_.m1_encoder_command_result.status;
}
for (retry = 0; retry < maxCommandRetries_; retry++) { int32_t RoboClaw::getM2Encoder() {
try { return g_sensor_value_group_.m2_encoder_command_result.value;
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 !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::cache_getM2Encoder] RETRY COUNT EXCEEDED"); int8_t RoboClaw::getM2EncoderStatus() {
throw new TRoboClawException( return g_sensor_value_group_.m2_encoder_command_result.status;
"[RoboClaw::cache_getM2Encoder] RETRY COUNT EXCEEDED");
} }
std::string RoboClaw::getVersion() { std::string RoboClaw::getVersion() {
@@ -1104,24 +730,37 @@ uint8_t RoboClaw::readByteWithTimeout2() {
void RoboClaw::readSensorGroup() { void RoboClaw::readSensorGroup() {
if (singleton() != nullptr) { 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_status = singleton()->cache_getErrorStatus();
g_sensor_value_group_.error_string = singleton()->cache_getErrorString(); g_sensor_value_group_.error_string = singleton()->cache_getErrorString();
g_sensor_value_group_.logic_battery_level = g_sensor_value_group_.logic_battery_level = logic_battery_level;
singleton()->cache_getLogicBatteryLevel(); g_sensor_value_group_.m1_encoder_command_result = m1_encoder_command_result;
g_sensor_value_group_.m1_encoder = singleton()->cache_getM1Encoder(); g_sensor_value_group_.m1_velocity = m1_speed;
g_sensor_value_group_.m1_encoder_command_result = g_sensor_value_group_.m2_encoder_command_result = m2_encoder_command_result;
singleton()->cache_getEncoderCommandResult(kGETM1ENC); g_sensor_value_group_.m2_velocity = m2_speed;
g_sensor_value_group_.m1_pidq = singleton()->cache_getPIDQ(kGETM1PID); g_sensor_value_group_.main_battery_level = main_battery_level;
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();
// Call getMotorCurrents before getMotorAlarms; // Call getMotorCurrents before getMotorAlarms;
g_sensor_value_group_.motor_currents = 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++) { for (uint8_t i = 0; i < cnt; i++) {
uint8_t byte = va_arg(marker, int); 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); updateCrc(crc, byte);
writeByte(byte); writeByte(byte);
} }
@@ -1357,5 +982,4 @@ void RoboClaw::writeN2(bool sendCRC, uint8_t cnt, ...) {
RoboClaw *RoboClaw::singleton() { return g_singleton; } RoboClaw *RoboClaw::singleton() { return g_singleton; }
RoboClaw::SensorValueGroup RoboClaw::g_sensor_value_group_;
RoboClaw *RoboClaw::g_singleton = nullptr; RoboClaw *RoboClaw::g_singleton = nullptr;