Ongoing encoder work

src/drivers/roboclaw/RoboClaw.cpp

@@ -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();
@@ -147,68 +151,113 @@ RoboClaw::~RoboClaw()
 
 void RoboClaw::Run()
 {
-	readEncoder(MOTOR_1);
+	readEncoder();
-	readEncoder(MOTOR_2);
+	//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) {
+	if (nread < 10) {
-		PX4_ERR("Error reading RoboClaw encoders: %d", nread);
+		PX4_ERR("Error reading encoders: %d", nread);
 		return -1;
 	}
 
-	uint32_t count = 0;
+	int32_t count;
-	auto countBytes = (uint8_t *)(&count);
+	uint8_t *count_bytes;
-	countBytes[3] = rbuf[0];
-	countBytes[2] = rbuf[1];
-	countBytes[1] = rbuf[2];
-	countBytes[0] = rbuf[3];
-	uint8_t status = rbuf[4];
 
-	int overflowFlag = 0;
+	for (int motor = 0; motor <= 1; motor++) {
+		count = 0;
+		count_bytes = &rbuff[motor * 4];
 
-	if (status & STATUS_UNDERFLOW) {
+		// Data from the roboclaw is big-endian. This converts the bytes to an integer, regardless of the
-		overflowFlag = -1;
+		// endianness of the system this code is running on.
-		PX4_INFO("=====UNDERFLOW=====");
+		for (int byte = 0; byte < 4; byte++) {
+			count = (count << 8) + count_bytes[byte];
+		}
 
-	} else if (status & STATUS_OVERFLOW) {
+		int overflow = 0;
-		PX4_INFO("=====OVERFLOW=====");
-		overflowFlag = +1;
-	}
 
-	int32_t *encoderCount;
+		if (count - _lastEncoderCount[motor] > OVERFLOW_AMOUNT / 2) {
-	int32_t *revolutions;
+			overflow = -1;
-	int32_t *overflow;
 
-	if (motor == MOTOR_1) {
+		} else if (_lastEncoderCount[motor] - count > OVERFLOW_AMOUNT / 2) {
-		encoderCount = &_motor1EncoderCounts;
+			overflow = +1;
-		revolutions = &_motor1Revolutions;
+		}
-		overflow = &_motor1Overflow;
 
-	} else {
+		int64_t adjusted_count = int64_t(count) + (overflow * int64_t(OVERFLOW_AMOUNT));
-		encoderCount = &_motor2EncoderCounts;
+		int32_t diff = int32_t(adjusted_count - int64_t(_lastEncoderCount[motor]));
-		revolutions = &_motor2Revolutions;
+		_localPosition[motor] += diff;
-		overflow = &_motor2Overflow;
+		_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)
@@ -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.readEncoder(RoboClaw::MOTOR_2);
+		//roboclaw.readEncoder(RoboClaw::MOTOR_2);
 		roboclaw.printStatus(buf, 200);
 		printf("%s", buf);
 	}
@@ -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.readEncoder(RoboClaw::MOTOR_2);
+		//roboclaw.readEncoder(RoboClaw::MOTOR_2);
 		roboclaw.printStatus(buf, 200);
 		printf("%s", buf);
 	}

src/drivers/roboclaw/RoboClaw.hpp

@@ -137,7 +137,7 @@ public:
 	/**
 	 * read data from serial
 	 */
-	int readEncoder(e_motor motor);
+	int readEncoder();
 
 	/**
 	 * print status
@@ -174,6 +174,7 @@ private:
 		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,
@@ -208,12 +209,19 @@ private:
 	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