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-18 18:57:53 -07:00
parent 0b293e2e2c
commit df421a8b79
2 changed files with 44 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
include/roboclaw.h
View File

@@ -429,7 +429,8 @@ private:
}
void showLog() {
RCUTILS_LOG_INFO("%s, READ: %s", write_log_, read_log_);
RCUTILS_LOG_INFO("[RoboClaw::DebugLog] %s, READ: %s", write_log_,
read_log_);
read_log_[0] = '\0';
next_read_log_index_ = 0;
write_log_[0] = '\0';
@@ -497,8 +498,6 @@ private:
responseCrc |= datum;
if (responseCrc == crc) {
version_ = version.str();
// roboclaw_.debug_log_.appendToReadLog("RR%d", 0);
// roboclaw_.debug_log_.appendToWriteLog("WW%d", 0);
roboclaw_.debug_log_.showLog();
return;
} else {
@@ -554,6 +553,44 @@ private:
long value_;
};
class CmdReadStatus : public Cmd {
public:
CmdReadStatus(RoboClaw &roboclaw, unsigned short &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);
unsigned short result = (unsigned short)roboclaw_.getULongCont(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", result);
roboclaw_.debug_log_.showLog();
status_ = result;
return;
} else {
RCUTILS_LOG_ERROR(
"[RoboClaw::cache_getErrorStatus] invalid CRC expected: 0x%02X, "
"got: "
"0x%02X",
crc, responseCrc);
}
} catch (...) {
RCUTILS_LOG_ERROR(
"[RoboClaw::cache_getErrorStatus] Uncaught exception !!!");
}
};
unsigned short &status_;
};
friend class Cmd; // Make Cmd a friend class of RoboClaw
protected:

39
src/roboclaw.cpp
View File

@@ -383,41 +383,10 @@ uint16_t RoboClaw::getErrorStatus() {
}
uint16_t RoboClaw::cache_getErrorStatus() {
for (int retry = 0; retry < maxCommandRetries_; retry++) {
try {
uint16_t crc = 0;
updateCrc(crc, portAddress_);
updateCrc(crc, kGETERROR);
writeN(false, 2, portAddress_, kGETERROR);
unsigned short result = (unsigned short)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_getErrorStatus] invalid CRC expected: 0x%02X, "
"got: "
"0x%02X",
crc, responseCrc);
}
} catch (TRoboClawException *e) {
RCUTILS_LOG_ERROR(
"[RoboClaw::cache_getErrorStatus] Exception: %s, retry number: %d",
e->what(), retry);
} catch (...) {
RCUTILS_LOG_ERROR(
"[RoboClaw::cache_getErrorStatus] Uncaught exception !!!");
}
}
RCUTILS_LOG_ERROR("[RoboClaw::cache_getErrorStatus] RETRY COUNT EXCEEDED");
throw new TRoboClawException(
"[RoboClaw::cache_getErrorStatus] RETRY COUNT EXCEEDED");
unsigned short result;
CmdReadStatus cmd(*this, result);
cmd.execute();
return result;
}
std::string RoboClaw::getErrorString() {