Skip to content

Commit

Permalink
Ongoing encoder work
Browse files Browse the repository at this point in the history
  • Loading branch information
@julianoes
Timothy Scott authored and julianoes committed Aug 5, 2019
1 parent 6ea03be commit 0b3f636
Show file tree
Hide file tree
Showing 2 changed files with 105 additions and 48 deletions.
143 changes: 96 additions & 47 deletions src/drivers/roboclaw/RoboClaw.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +95,11 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, uint16_t pulsesPerR
_motor2EncoderCounts(0),
_motor2Revolutions(0),
_motor2Overflow(0),
_motor2Speed(0)
_motor2Speed(0),
_lastEncoderCount{0, 0},
_localPosition{0, 0},
_revolutions{0, 0}

{
// setup control polling
_controlPoll.fd = _actuators.getHandle();
Expand Down Expand Up @@ -147,68 +151,113 @@ RoboClaw::~RoboClaw()

void RoboClaw::Run()
{
readEncoder(MOTOR_1);
readEncoder(MOTOR_2);
readEncoder();
//readEncoder(MOTOR_2);

PX4_INFO("Motor1: (%d, %d), Motor2: (%d, %d)", _motor1EncoderCounts, _motor1Revolutions, _motor2EncoderCounts,
_motor2Revolutions);
}

int RoboClaw::readEncoder(e_motor motor)
int RoboClaw::readEncoder()
{
e_command cmd = motor == MOTOR_1 ? CMD_READ_ENCODER_1 : CMD_READ_ENCODER_2;
uint8_t rbuf[7];

int nread = _transaction(cmd, nullptr, 0, rbuf, 7, false, true);
uint8_t rbuff[10];
int nread = _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff[0], 10, false, true);

if (nread < 7) {
PX4_ERR("Error reading RoboClaw encoders: %d", nread);
if (nread < 10) {
PX4_ERR("Error reading encoders: %d", nread);
return -1;
}

uint32_t count = 0;
auto countBytes = (uint8_t *)(&count);
countBytes[3] = rbuf[0];
countBytes[2] = rbuf[1];
countBytes[1] = rbuf[2];
countBytes[0] = rbuf[3];
uint8_t status = rbuf[4];
int32_t count;
uint8_t *count_bytes;

int overflowFlag = 0;
for (int motor = 0; motor <= 1; motor++) {
count = 0;
count_bytes = &rbuff[motor * 4];

if (status & STATUS_UNDERFLOW) {
overflowFlag = -1;
PX4_INFO("=====UNDERFLOW=====");
// Data from the roboclaw is big-endian. This converts the bytes to an integer, regardless of the
// endianness of the system this code is running on.
for (int byte = 0; byte < 4; byte++) {
count = (count << 8) + count_bytes[byte];
}

} else if (status & STATUS_OVERFLOW) {
PX4_INFO("=====OVERFLOW=====");
overflowFlag = +1;
}
int overflow = 0;

int32_t *encoderCount;
int32_t *revolutions;
int32_t *overflow;
if (count - _lastEncoderCount[motor] > OVERFLOW_AMOUNT / 2) {
overflow = -1;

if (motor == MOTOR_1) {
encoderCount = &_motor1EncoderCounts;
revolutions = &_motor1Revolutions;
overflow = &_motor1Overflow;
} else if (_lastEncoderCount[motor] - count > OVERFLOW_AMOUNT / 2) {
overflow = +1;
}

} else {
encoderCount = &_motor2EncoderCounts;
revolutions = &_motor2Revolutions;
overflow = &_motor2Overflow;
}
int64_t adjusted_count = int64_t(count) + (overflow * int64_t(OVERFLOW_AMOUNT));
int32_t diff = int32_t(adjusted_count - int64_t(_lastEncoderCount[motor]));
_localPosition[motor] += diff;
_revolutions[motor] += _localPosition[motor] / _pulsesPerRev;
_localPosition[motor] %= _pulsesPerRev;
PX4_INFO("Motor %d - LastCount: %7d, Count: %7d, Overflow: %+1d, adjusted count: %+8lld, local: %4d, revs: %2lld",
motor, _lastEncoderCount[motor], count, overflow, adjusted_count, _localPosition[motor], _revolutions[motor]);
_lastEncoderCount[motor] = count;

PX4_INFO("COUNT: %08X STATUS: %02X", count, status);

*overflow += overflowFlag;
int64_t totalCounts = int64_t(count) + (int64_t(*overflow) * OVERFLOW_AMOUNT);
PX4_INFO("Total counts: %lld", totalCounts);
*encoderCount = int32_t(totalCounts % _pulsesPerRev);
*revolutions = int32_t(totalCounts / _pulsesPerRev);
}

return 0;
return 1;

// e_command cmd = motor == MOTOR_1 ? CMD_READ_ENCODER_1 : CMD_READ_ENCODER_2;
// uint8_t rbuf[7];
//
// int nread = _transaction(cmd, nullptr, 0, rbuf, 7, false, true);
//
// if (nread < 7) {
// PX4_ERR("Error reading RoboClaw encoders: %d", nread);
// return -1;
// }
//
// uint32_t count = 0;
// auto countBytes = (uint8_t *)(&count);
// countBytes[3] = rbuf[0];
// countBytes[2] = rbuf[1];
// countBytes[1] = rbuf[2];
// countBytes[0] = rbuf[3];
// uint8_t status = rbuf[4];
//
// int overflowFlag = 0;
//
// if (status & STATUS_UNDERFLOW) {
// overflowFlag = -1;
// PX4_INFO("=====UNDERFLOW=====");
//
// } else if (status & STATUS_OVERFLOW) {
// PX4_INFO("=====OVERFLOW=====");
// overflowFlag = +1;
// }
//
// int32_t *encoderCount;
// int32_t *revolutions;
// int32_t *overflow;
//
// if (motor == MOTOR_1) {
// encoderCount = &_motor1EncoderCounts;
// revolutions = &_motor1Revolutions;
// overflow = &_motor1Overflow;
//
// } else {
// encoderCount = &_motor2EncoderCounts;
// revolutions = &_motor2Revolutions;
// overflow = &_motor2Overflow;
// }
//
// PX4_INFO("COUNT: %08X STATUS: %02X", count, status);
//
// *overflow += overflowFlag;
// int64_t totalCounts = int64_t(count) + (int64_t(*overflow) * OVERFLOW_AMOUNT);
// PX4_INFO("Total counts: %lld", totalCounts);
// *encoderCount = int32_t(totalCounts % _pulsesPerRev);
// *revolutions = int32_t(totalCounts / _pulsesPerRev);
//
// return 0;
}

void RoboClaw::printStatus(char *string, size_t n)
Expand Down Expand Up @@ -491,8 +540,8 @@ int RoboClaw::roboclawTest(int argc, char *argv[])

for (int i = 0; i < 10; i++) {
usleep(100000);
roboclaw.readEncoder(RoboClaw::MOTOR_1);
roboclaw.readEncoder(RoboClaw::MOTOR_2);
roboclaw.readEncoder();
//roboclaw.readEncoder(RoboClaw::MOTOR_2);
roboclaw.printStatus(buf, 200);
printf("%s", buf);
}
Expand All @@ -502,8 +551,8 @@ int RoboClaw::roboclawTest(int argc, char *argv[])

for (int i = 0; i < 10; i++) {
usleep(100000);
roboclaw.readEncoder(RoboClaw::MOTOR_1);
roboclaw.readEncoder(RoboClaw::MOTOR_2);
roboclaw.readEncoder();
//roboclaw.readEncoder(RoboClaw::MOTOR_2);
roboclaw.printStatus(buf, 200);
printf("%s", buf);
}
Expand Down
10 changes: 9 additions & 1 deletion src/drivers/roboclaw/RoboClaw.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,7 +137,7 @@ class RoboClaw : public px4::ScheduledWorkItem
/**
* read data from serial
*/
int readEncoder(e_motor motor);
int readEncoder();

/**
* print status
Expand Down Expand Up @@ -174,6 +174,7 @@ class RoboClaw : public px4::ScheduledWorkItem
CMD_READ_ENCODER_1 = 16,
CMD_READ_ENCODER_2 = 17,
CMD_RESET_ENCODERS = 20,
CMD_READ_BOTH_ENCODERS = 78,

// advanced motor control
CMD_READ_SPEED_HIRES_1 = 30,
Expand Down Expand Up @@ -208,12 +209,19 @@ class RoboClaw : public px4::ScheduledWorkItem
int32_t _motor2Overflow;
float _motor2Speed;

int32_t _lastEncoderCount[2];
int32_t _localPosition[2];
int64_t _revolutions[2];


// private methods
uint16_t _sumBytes(uint8_t *buf, size_t n, uint16_t init = 0);
int _sendUnsigned7Bit(e_command command, float data);
int _sendSigned16Bit(e_command command, float data);
int _sendNothing(e_command);

int32_t _bytesToInt(uint8_t *bytes);

/**
* Perform a round-trip write and read.
* @param cmd Command to send to the Roboclaw
Expand Down

0 comments on commit 0b3f636

Please sign in to comment.