Roboclaw encoders/ dutycycledrive complete.

3 files changed, 116 insertions, 54 deletions

diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index aecc511ae..3bbd7f48f 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -59,15 +59,19 @@
 
 uint8_t RoboClaw::checksum_mask = 0x7f;
 
-RoboClaw::RoboClaw(const char *deviceName, uint16_t address):
+RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
+		uint16_t pulsesPerRev):
 	_address(address),
+	_pulsesPerRev(pulsesPerRev),
 	_uart(0),
 	_controlPoll(),
 	_actuators(NULL, ORB_ID(actuator_controls_0), 20),
 	_motor1Position(0),
 	_motor1Speed(0),
+	_motor1Overflow(0),
 	_motor2Position(0),
-	_motor2Speed(0)
+	_motor2Speed(0),
+	_motor2Overflow(0)
 {
 	// setup control polling
 	_controlPoll.fd = _actuators.getHandle();
@@ -90,6 +94,13 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address):
 
 	// setup default settings, reset encoders
 	resetEncoders();
+
+	// XXX roboclaw gives 128 as first several csums
+	// have to read a couple of messages first
+	for (int i=0;i<5;i++) {
+		if (readEncoder(MOTOR_1) > 0) break;
+		usleep(3000);
+	}	
 }
 
 RoboClaw::~RoboClaw()
@@ -107,8 +118,18 @@ int RoboClaw::readEncoder(e_motor motor)
 	} else if (motor == MOTOR_2) {
 		_sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum);
 	}
-	uint8_t rbuf[6];
-	int ret = read(_uart, rbuf, 6);
+	uint8_t rbuf[50];
+	usleep(5000);
+	int nread = read(_uart, rbuf, 50);
+	if (nread < 6) { 
+		printf("failed to read\n");
+		return -1;
+	}
+	//printf("received: \n");
+	//for (int i=0;i<nread;i++) {
+		//printf("%d\t", rbuf[i]);
+	//}
+	//printf("\n");
 	uint32_t count = 0;
 	uint8_t * countBytes = (uint8_t *)(&count);
 	countBytes[3] = rbuf[0];
@@ -116,34 +137,46 @@ int RoboClaw::readEncoder(e_motor motor)
 	countBytes[1] = rbuf[2];
 	countBytes[0] = rbuf[3];
 	uint8_t status = rbuf[4];
-	if (((sum + _sumBytes(rbuf,5)) & checksum_mask) 
-			== rbuf[5]) return -1;
+	uint8_t checksum = rbuf[5];
+	uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & 
+		checksum_mask;
+	// check if checksum is valid
+	if (checksum != checksum_computed) {
+		printf("checksum failed: expected %d got %d\n",
+				checksum, checksum_computed);
+		return -1;
+	}
 	int overFlow = 0;
+
+	if (status & STATUS_REVERSE) {
+		//printf("roboclaw: reverse\n");
+	}
+
 	if (status & STATUS_UNDERFLOW) {
-		printf("roboclaw: underflow\n");
+		//printf("roboclaw: underflow\n");
 		overFlow = -1;
 	} else if (status & STATUS_OVERFLOW) {
-		printf("roboclaw: overflow\n");
+		//printf("roboclaw: overflow\n");
 		overFlow = +1;
 	}
+
 	static int64_t overflowAmount = 0x100000000LL;
 	if (motor == MOTOR_1) {
 		_motor1Overflow += overFlow;
-		_motor1Position = count + 
-			_motor1Overflow*_motor1Position;
+		_motor1Position = float(int64_t(count) + 
+			_motor1Overflow*overflowAmount)/_pulsesPerRev;
 	} else if (motor == MOTOR_2) {
 		_motor2Overflow += overFlow;
-		_motor2Position = count + 
-			_motor2Overflow*_motor2Position;
+		_motor2Position = float(int64_t(count) + 
+			_motor2Overflow*overflowAmount)/_pulsesPerRev;
 	}
-	return ret;
+	return 0;
 }
 
 void RoboClaw::printStatus(char *string, size_t n)
 {
 	snprintf(string, n,
-		 "motor 1 position:\t%10.2f\tspeed:\t%10.2f\n"
-		 "motor 2 position:\t%10.2f\tspeed:\t%10.2f\n",
+		 "pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n",
 		 double(getMotorPosition(MOTOR_1)),
 		 double(getMotorSpeed(MOTOR_1)),
 		 double(getMotorPosition(MOTOR_2)),
@@ -178,17 +211,18 @@ int RoboClaw::setMotorSpeed(e_motor motor, float value)
 	// send command
 	if (motor == MOTOR_1) {
 		if (value > 0) {
-			_sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum);
+			return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum);
 		} else {
-			_sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum);
+			return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum);
 		}
 	} else if (motor == MOTOR_2) {
 		if (value > 0) {
-			_sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum);
+			return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum);
 		} else {
-			_sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum);
+			return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum);
 		}
 	}
+	return -1;
 }
 
 int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
@@ -200,12 +234,13 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
 	int16_t duty = 1500*value;
 	// send command
 	if (motor == MOTOR_1) {
-		_sendCommand(CMD_SIGNED_DUTYCYCLE_1,
+		return _sendCommand(CMD_SIGNED_DUTYCYCLE_1,
 				(uint8_t *)(&duty), 2, sum);
 	} else if (motor == MOTOR_2) {
-		_sendCommand(CMD_SIGNED_DUTYCYCLE_2,
+		return _sendCommand(CMD_SIGNED_DUTYCYCLE_2,
 				(uint8_t *)(&duty), 2, sum);
 	}
+	return -1;
 }
 
 int RoboClaw::resetEncoders() 
@@ -215,13 +250,13 @@ int RoboClaw::resetEncoders()
 			nullptr, 0, sum);
 }
 
-void RoboClaw::update()
+int RoboClaw::update()
 {
 	// wait for an actuator publication,
 	// check for exit condition every second
 	// note "::poll" is required to distinguish global
 	// poll from member function for driver
-	if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error
+	if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error
 
 	// if new data, send to motors
 	if (_actuators.updated()) {
@@ -229,56 +264,68 @@ void RoboClaw::update()
 		setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]);
 		setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]);
 	}
+	return 0;
 }
 
 uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
 {
 	uint16_t sum = 0;
-	printf("sum\n");
+	//printf("sum\n");
 	for (size_t i=0;i<n;i++) {
 		sum += buf[i];
-		printf("%d\t", buf[i]);
+		//printf("%d\t", buf[i]);
 	}
-	printf("total sum %d\n", sum);
-	printf("\n");
+	//printf("total sum %d\n", sum);
 	return sum;
 }
 
 int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, 
 		size_t n_data, uint16_t & prev_sum)
 {
-	tcflush(_uart, TCIOFLUSH);
+	tcflush(_uart, TCIOFLUSH); // flush  buffers
 	uint8_t buf[n_data + 3];
 	buf[0] = _address;
 	buf[1] = cmd;
-	for (int i=0;i<n_data;i++) {
+	for (size_t i=0;i<n_data;i++) {
 		buf[i+2] = data[n_data - i - 1]; // MSB
 	}
 	uint16_t sum = _sumBytes(buf, n_data + 2);
 	prev_sum += sum;
 	buf[n_data + 2] = sum & checksum_mask;
-	printf("\nmessage:\n");
-	for (int i=0;i<n_data+3;i++) {
-		printf("%d\t", buf[i]);
-	}
-	printf("\n");
+	//printf("\nmessage:\n");
+	//for (size_t i=0;i<n_data+3;i++) {
+		//printf("%d\t", buf[i]);
+	//}
+	//printf("\n");
 	return write(_uart, buf, n_data + 3);
 }
 
-int roboclawTest(const char *deviceName, uint8_t address)
+int roboclawTest(const char *deviceName, uint8_t address, 
+		uint16_t pulsesPerRev)
 {
 	printf("roboclaw test: starting\n");
 
 	// setup
-	RoboClaw roboclaw(deviceName, address);
+	RoboClaw roboclaw(deviceName, address, pulsesPerRev);
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3);
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, 0.3);
 	char buf[200];
 	for (int i=0; i<10; i++) {
-		usleep(1000000);
-		roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3);
-		//roboclaw.setMotorSpeed(RoboClaw::MOTOR_1, 0.3);
-		//roboclaw.readEncoder(RoboClaw::MOTOR_1);
-		roboclaw.printStatus(buf, 200);
-		printf("%s\n", buf);
+		usleep(100000);
+		roboclaw.readEncoder(RoboClaw::MOTOR_1);
+		roboclaw.readEncoder(RoboClaw::MOTOR_2);
+		roboclaw.printStatus(buf,200);
+		printf("%s", buf);
+	}
+
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, -0.3);
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, -0.3);
+	for (int i=0; i<10; i++) {
+		usleep(100000);
+		roboclaw.readEncoder(RoboClaw::MOTOR_1);
+		roboclaw.readEncoder(RoboClaw::MOTOR_2);
+		roboclaw.printStatus(buf,200);
+		printf("%s", buf);
 	}
 
 	printf("Test complete\n");
diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp
index 619c2289c..e9f35cf95 100644
--- a/src/drivers/roboclaw/RoboClaw.hpp
+++ b/src/drivers/roboclaw/RoboClaw.hpp
@@ -74,8 +74,11 @@ public:
 	 * 	serial port e.g. "/dev/ttyS2"
 	 * @param address the adddress  of the motor
 	 * 	(selectable on roboclaw)
+	 * @param pulsesPerRev # of encoder
+	 *  pulses per revolution of wheel
 	 */
-	RoboClaw(const char *deviceName, uint16_t address);
+	RoboClaw(const char *deviceName, uint16_t address, 
+			uint16_t pulsesPerRev);
 
 	/**
 	 * deconstructor
@@ -112,7 +115,7 @@ public:
 	 * main update loop that updates RoboClaw motor
 	 * dutycycle based on actuator publication
 	 */
-	void update();
+	int update();
 
 	/**
 	 * read data from serial
@@ -158,6 +161,7 @@ private:
 	static uint8_t checksum_mask;
 
 	uint16_t _address;
+	uint16_t _pulsesPerRev;
 
 	int _uart;
 
@@ -182,6 +186,7 @@ private:
 };
 
 // unit testing
-int roboclawTest(const char *deviceName, uint8_t address);
+int roboclawTest(const char *deviceName, uint8_t address,
+		uint16_t pulsesPerRev);
 
 // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp
index f37f71aac..44ed03254 100644
--- a/src/drivers/roboclaw/roboclaw_main.cpp
+++ b/src/drivers/roboclaw/roboclaw_main.cpp
@@ -114,17 +114,25 @@ int roboclaw_main(int argc, char *argv[])
 
 	} else if (!strcmp(argv[1], "test")) {
 
-		if (argc < 4) {
-			printf("usage: roboclaw test device address\n");
+		const char * deviceName = "/dev/ttyS2";
+		uint8_t address = 128;
+		uint16_t pulsesPerRev = 1200;
+
+		if (argc == 2) {
+			printf("testing with default settings\n");
+		} else if (argc != 4) {
+			printf("usage: roboclaw test device address pulses_per_rev\n");
 			exit(-1);
+		} else {
+			deviceName = argv[2];
+			address = strtoul(argv[3], nullptr, 0);
+			pulsesPerRev = strtoul(argv[4], nullptr, 0);
 		}
 
-		const char *deviceName = argv[2];
-		uint8_t address = strtoul(argv[3], nullptr, 0);
-
-		printf("device:\t%s\taddress:\t%d\n", deviceName, address);
+		printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
+			deviceName, address, pulsesPerRev);
 
-		roboclawTest(deviceName, address);
+		roboclawTest(deviceName, address, pulsesPerRev);
 		thread_should_exit = true;
 		exit(0);
 
@@ -163,11 +171,13 @@ int roboclaw_thread_main(int argc, char *argv[])
 
 	const char *deviceName = argv[1];
 	uint8_t address = strtoul(argv[2], nullptr, 0);
+	uint16_t pulsesPerRev = strtoul(argv[3], nullptr, 0);
 
-	printf("device:\t%s\taddress:\t%d\n", deviceName, address);
+	printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
+			deviceName, address, pulsesPerRev);
 
 	// start
-	RoboClaw roboclaw(deviceName, address);
+	RoboClaw roboclaw(deviceName, address, pulsesPerRev);
 
 	thread_running = true;