Close

Calibrating the system

A project log for Controll yer Roomba 600 series with ESP8266

Another ESP8266 project. If some hardware has an interface / API to connect to, it's waiting to be assimilated into the home-hive.

Simon JansenSimon Jansen 04/19/2022 at 21:000 Comments

Have I got results for you!

To calibrate the system I needed a way to do scripts. Move X forward, turn Y right, wait T, repeat i,  etc. 

To get calibration data, drive a figure 8, two squares. First leg should have the same heading as the last leg. Like this!:

I'm pretty proud of the fruits of my labour. First, the ability to do "scripting":

int Roomba632::ScriptHandler(){
	int16_t velocity, radius;
	uint8_t scriptDistance, scriptAngle, scriptTime;
	static uint8_t scriptRepeat;
	static int scriptState, p_programCounter; //program counter for scripts;
	static unsigned long scriptTimer, waitingTime;
	switch (scriptState){
		case RMIdle:	//0
			if (p_startScriptFlag){
				p_startScriptFlag = false;
				scriptState = RMStart;
			}
			break;
		case RMStart:	//1
			Serial.write(OCstart);
			scriptTimer = millis();
			scriptState = RMStarting;
			break;
		case RMStarting://2
			if ((millis() - scriptTimer) > COMMANDSTARTTIMEOUT){
				Serial.write(OCSafemode);
				p_programCounter= 0;	//Reset script
				scriptState = p_scriptPointer[p_programCounter];
				scriptTimer = millis();
			}
			break;
		case RMHalt:	//3 scriptable
			if ((millis() - scriptTimer) > COMMANDMODETIMEOUT){
				Serial.write(OCStop);
				scriptTimer = millis();
				scriptState = RMHalting;
			}
			break;
		case RMHalting:	//4
			if((millis() - scriptTimer) > COMMANDSTARTTIMEOUT){
				p_programCounter = 0;
				scriptState = RMIdle;
			}
			break;
		case RMDriveF:	//5 scriptable verb + noun
			if ((millis() - scriptTimer) > COMMANDMODETIMEOUT){
				scriptDistance = p_scriptPointer[p_programCounter + 1]; //Distance in steps of 25mm (gives maximum of 6.375m)
				waitingTime = scriptDistance * DISTANCESCALAR; 
				velocity = 250; //mm/s
				radius = 32767; //Drive straight
				SendDriveCommand(velocity, radius);
				scriptTimer = millis();
				scriptState = RMDrivingF;
			}
			break;
		case RMDrivingF://6
			if((millis() - scriptTimer) > waitingTime){
				p_programCounter = p_programCounter + 2;
				scriptState = p_scriptPointer[p_programCounter];
				scriptTimer = millis();
				debugVal = scriptState;
			}
			break;
		case RMDriveR:	//7 scriptable verb + noun
			if ((millis() - scriptTimer) > COMMANDMODETIMEOUT){
				scriptDistance = p_scriptPointer[p_programCounter + 1]; //Distance in steps of 25mm (gives maximum of 6.375m)
				waitingTime = scriptDistance * DISTANCESCALAR; 
				velocity = -250; //mm/s
				radius = 32767; //Drive straight
				SendDriveCommand(velocity, radius);
				scriptTimer = millis();
				scriptState = RMDrivingR;
			}
			break;
		case RMDrivingR://8
			if((millis() - scriptTimer) > waitingTime){
				p_programCounter = p_programCounter + 2;
				scriptState = p_scriptPointer[p_programCounter];
				scriptTimer = millis();
			}
			break;
		case RMTurnL:	//9 scriptable verb + noun
			if ((millis() - scriptTimer) > COMMANDMODETIMEOUT){
				scriptAngle = p_scriptPointer[p_programCounter +1]; //Angle in 256 steps for full circle (quarter turn = 64)
				waitingTime = scriptAngle * TURNINGSPEED;//quarter turn should give 1570ms
				velocity = 256; //mm/s 
				radius = 256; //radius in mm's
				SendDriveCommand(velocity, radius);
				scriptTimer = millis();
				scriptState = RMTurningL;
			}
			break;
		case RMTurningL://10
			if((millis() - scriptTimer) > waitingTime){
				p_programCounter = p_programCounter + 2;
				scriptState = p_scriptPointer[p_programCounter];
				scriptTimer = millis();
			}
			break;
		case RMTurnR:	//11 scriptable verb + noun
			if ((millis() - scriptTimer) > COMMANDMODETIMEOUT){
				scriptAngle = p_scriptPointer[p_programCounter +1]; //Angle in 256 steps for full circle (quarter turn = 64)
				waitingTime = scriptAngle * (TURNINGSPEED);//quarter turn should give 1570ms
				velocity = 256 - TURNINGOFFSET; //mm/s 
				radius = -256; //radius in mm's
				SendDriveCommand(velocity, radius);
				scriptTimer = millis();
				scriptState = RMTurningR;
			}
			break;
		case RMTurningR://12
			if((millis() - scriptTimer) > waitingTime){
				p_programCounter = p_programCounter + 2;
				scriptState = p_scriptPointer[p_programCounter];
				scriptTimer = millis();
			}
			break;
		case RMWait:	//13 scriptable verb + noun
			if ((millis() - scriptTimer) > COMMANDMODETIMEOUT){
				SendDriveCommand(0, 0);
				scriptTime = p_scriptPointer[p_programCounter +1]; //Wating time in steps of 128ms for maximum wating time of ~32,5 seconds
				waitingTime = scriptTime * 128;
				scriptState = RMWaiting;
				scriptTimer = millis();
			}
			break;
		case RMWaiting:	//14
			if((millis() - scriptTimer) > waitingTime){
				p_programCounter = p_programCounter + 2;
				scriptState = p_scriptPointer[p_programCounter];
				scriptTimer = millis();
			}
			break;
		case RMGoto: //15 Will go on forever!
			p_programCounter = p_scriptPointer[p_programCounter + 1];
			scriptState = p_scriptPointer[p_programCounter];
			scriptTimer = millis();
			break;
		case RMRepeat: //16 repeat for total of numbered runs
			if (scriptRepeat == 0){
				scriptRepeat = p_scriptPointer[p_programCounter + 1];
				scriptState = p_scriptPointer[0];
			}
			else if(scriptRepeat != 1){
				scriptRepeat--;
				scriptState = p_scriptPointer[0];
			}
			else if(scriptRepeat == 1){
				scriptState = RMHalt;
			}
			p_programCounter = 0;
			scriptTimer = millis();
			break;
		default:
			//Reset
			scriptState = RMHalt;
			break;
	}
	return scriptState;
}
void Roomba632::SendDriveCommand(int16_t velocity, int16_t radius){
	Serial.write(OCDrive);
	Serial.write(velocity >> 8);
	Serial.write(velocity);
	Serial.write(radius >> 8);
	Serial.write(radius);
}

 It's using pointers and everything! (It's added to the newly uploaded library files. Update includes much more improvements. All timers are now overflow proof etc)

With this, I can run the following script:

 uint8_t Square[35] = {
  RMDriveF, 20, //50cm           
  RMTurnL, 64,  //quarter turn L  
  RMDriveF, 20, //50cm            
  RMTurnL, 64,  //quarter turn L  
  RMDriveF, 20, //50cm            
  RMTurnL, 64,  //quarter turn L  
  RMDriveF, 20, //50cm
  RMTurnL, 64,  //quarter turn L
  RMDriveF, 20, //50cm
  RMTurnR, 64,  //quarter turn R
  RMDriveF, 20, //50cm
  RMTurnR, 64,  //quarter turn R
  RMDriveF, 20, //50cm
  RMTurnR, 64,  //quarter turn R
  RMDriveF, 20, //50cm
  RMTurnR, 64,  //quarter turn R
  RMDriveF, 20, //50cm
  RMHalt        //Stop
  };

Of course, this didn't give me a the correct result right away. I was able to bracket the values until I got the figure eight I was looking for.  Some specialised callibrationtape on the floor to compare starting and finishing position and heading.

Then, log the data for post processing:

X, Y data calculated real time by the ESP8266, logged via MQTT and viewed in Excel:

Now, I can do post-processing on the data in Octave to see if I can improve:

Raw, post-process:

Best result:

Now, I have calibrated values to use onboard for post-process AND real-time positioning. Updating the lib, with these values:

Maybe I'll do some more tweaking, but I don't think I'll get it much better than this. There are limits to the accuracy of the system. For better results, I'll have to integrate the IMU data. 

Discussions