Oh! Glorious dots!

I've got a tracking algorithm working! There were a few adjustments needed. Firstly, the encoders are signed 16 bits, so they rollover into negative numbers.

To make sure this rollover won't effect the calculations, first calculate the encodercount since previous and do this all with signed 16 bit numbers. Now we can use this number to calculate theta which is a floating point number.

Then we take the Sin-1 of the difference in distance driven by both wheels, divided by wheelbase which gives a approximation of the turned angle since previous.

 Adding this angle to the previous angle gives an angle in the "world" coordinate system.

This can be used to translate X and Y.

void Roomba632::UpdateOdometry(){
	if((millis() - p_odometryTimer)>ODOMETRYINTERVAL){
		p_odometryTimer = millis();
		static int16_t encoderRPosPrev = 0;
		static int16_t encoderLPosPrev = 0;
		int16_t encoderRDelta = encoderRight - encoderRPosPrev;
		int16_t encoderLDelta = encoderLeft - encoderLPosPrev;
		float SR = DISTANCEPERCOUNT * (encoderRDelta);
		float SL = DISTANCEPERCOUNT * (encoderLDelta);
		float meanDistance = (SL + SR)/2;
		encoderRPosPrev = encoderRight;
		encoderLPosPrev = encoderLeft;
		theta += atan((SR - SL) / WHEELDISTANCE);
		if(theta > 6.28){
			theta -= 6.28;
		}
		else if(theta < -6.28){
			theta += 6.28;
		}
		Xcoo += meanDistance * cos(theta);           
		Ycoo += meanDistance * sin(theta);
	}
}

The "tracks" really represent the actual path driven. There is of course an error in this approximation. This is very clear when the roomba only drives away from it's dock, turns in place and docks again. The "outgoing" line (top one here) should be on top of the "docking" line (bottom line).  

To correct this, I first have to understand the source of the error. Is this a physical thing? One wheel bigger than the other, wheel base measurement? Or is it akin to the calculation / algorithm?

I could just add some offsets and scale corrections in the calculation of theta, but  I should probably model the problem in Octave / Matlab.