I've incorporated the software to use the IMU into the main sketch. In a previous log I already figured out how to use it and what to adjust:

https://hackaday.io/project/183524-controll-yer-roomba-600-series-with-esp8266/log/202585-adding-direction-to-the-project

In previous attempts to use the MPU6050 everything got unstable. The main loop would only get about 400 runs/second. That's way too little for a usable system with stable communication.

The roomba's datastream puts out 66 messages a second. Each message containing approximately 50 bytes for a set of sensorpackets. We evaluate 1 serial byte every loop. So we need at least 3.500 loops a second to interpret the datastream. 

The GitHub repository has some clues: https://github.com/jrowberg/i2cdevlib/issues/657

This seems like a weird decision to me, but okay. And when I use the interrupt flag as intended, I get about 10.000 loops a second! 

void IMUHandler() {
  if (!dmpReady) return;
  // read a packet from FIFO
  if (mpuInterrupt) {
    mpuInterrupt = false; //Clear interrupt flag
    if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
      // display Yaw Pitch Roll in degrees
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetAccel(&aa, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
      roomba.AuxYaw = ypr[0];
    }
  }
}

Solved!

I then made arrangements so the positioning code can use the IMU-data. Along with some more tweaks for ease of use. I added "on the fly" calibration of the IMU and output of the offset-values over MQTT. Now I can hardcode the starting offsets in the sketch. 

I noticed the ESP8266 will crash hard when trying to calibrate with wrong starting values or in a different orientation. It will crash a first time, but the MPU6050 will have received different offsets. A reset and therefore a second pass at the calibration routine works. And the offsets we get over MQTT can be uploaded in the next firmware and all is well.

The result of a full cleaning session, position calculation with the IMU for orientation and encoders for distance. Logging 5 times a second cleaning for ~20mins. Green arrow = starting position, Red dots = finish and docking. Those two should line up: 

This is clearly better than before. I can kinda figure out where my table and chairs are. But when I did a full cleaning session of an hour:

Question now is... is this better?? Most important is that any source of the large error is not stochastic, but systemic and therefore correctable. After that we can solve the minor stochastic errors.

And we have everything in place to tweak the system and I'm confident we can get there.

So now we've got: 

• Stable software with Over The Air updates;
• Stable power and WiFi connection;
• Roomba encoder and sensordata every 15ms;
• Properly callibrated and fused IMU-data every 10ms;
• (async) MQTT interfacing;
• Ways to calculate real time positioning;
• Ability to do scripting;

I will upload the Arduino sketch I have so far. 

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. 

It's never not working.. it's an iterative process..

I've got some datasets with encoder values of "cleaning" sessions. I used GNU octave to make plots using the same algorithm the roomba uses. Only this time "post-process".

This way I can mess around with values to see if I can improve the accuracy of the system. 

The octave code (M-file):

clear();
#DISTANCEPERCOUNT = pi * 72.0 / 508.8
#DISTANCEPERCOUNT = 0.48
DISTANCEPERCOUNT = 0.44456499814949904317867595046408
#WHEELDISTANCE = 235.0
WHEELDISTANCE = 235.0
FILE = '**\FullClean.csv'

fid=fopen(FILE,'r');
nlines = fskipl (fid, Inf)
frewind (fid);
A=fscanf(fid,'%d,%d',[2 nlines+1]);

encoderRPosPrev = A(1);
encoderLPosPrev = A(2);
theta = 0;
Xcoo = 0;
Ycoo = 0;
Xrecord(1) = 0;
Yrecord(1) = 0;

for record = A
  encoderRight=record(2);
  encoderLeft=record(1);

  encoderRDelta = encoderRight-encoderRPosPrev;
  if (encoderRDelta > 32768)
    encoderRDelta = (encoderRight - 32768) - (encoderRPosPrev+32768);
  elseif (encoderRDelta < -32768)
    encoderRDelta = (encoderRight + 32768) - (encoderRPosPrev-32768);
  endif

  encoderLDelta = encoderLeft-encoderLPosPrev;
  if (encoderLDelta > 32768)
    encoderLDelta = (encoderLeft - 32768) - (encoderLPosPrev+32768);
  elseif (encoderLDelta < -32768)
    encoderLDelta = (encoderLeft + 32768) - (encoderLPosPrev-32768);
  endif
  SR = DISTANCEPERCOUNT * encoderRDelta;
  SL = DISTANCEPERCOUNT * encoderLDelta;
  meanDistance = (SL + SR)/2;
  encoderRPosPrev = encoderRight;
  encoderLPosPrev = encoderLeft;
  theta += atan((SR - SL) / WHEELDISTANCE);
  if(theta > 6.28)
    theta -= 6.28;
  elseif(theta < -6.28)
    theta += 6.28;
  endif
  Xcoo += meanDistance * cos(theta);
  Ycoo += meanDistance * sin(theta);
  Xrecord(end+1) = Xcoo;
  Yrecord(end+1) = Ycoo;
endfor
Xplot = Xrecord';
Yplot = Yrecord';
scatter(-Yplot,Xplot,1);

It's not the prettiest, but it works. I had the most difficult time implementing the int16 wraparound. In the end I decided to kinda brute force it.

And some results:

Wheel Distance: 220mm / DistancePerCount: 0.44mm

Wheel Distance: 235mm / DistancePerCount: 0.44mm

Wheel Distance: 250mm / DistancePerCount: 0.44mm

Wheel Distance: 250mm / DistancePerCount: 0.44mm

As you can see, I have some control over the results, but I don't know if it will be enough.

Maybe the whole system is just too noisy to get good results. I think I'll make some driving scripts to get better defined datasets. Drive in a square for starters. The first leg overlapping with the last leg so theta should be equal. This way, I have a closing error and a result to minimise. 

I really want to see what I can get out of the wheel encoders before integrating the IMU. 

Well, I'm having fun :)

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.

The tedious part is mostly done. Let the fun begin!

I now have a robust system for monitoring. Basically I have a data-stream running in a MQTT-topic. I then run mosquitto_sub on that topic with '| tee log.json' on a pi which gives me a nice log. For a quick result, I give the JSON-file a header:

{ "records": [

 Separate all records with a comma and wrap around with   

  ]
}

 I import this in Excel for some plotting. This also gives me the opportunity to do some filtering and I get a very nice idea of the full system. This is a 10 hr log with a cleaning cycle at the beginning:

Cool! Love me some plots :) And most importantly, the encoders while driving/cleaning:

I see/learn a few things:

• The encoders are 16 bit signed values that wrap around;
• The data-stream provides enough throughput (interval 15 ms);
• The interval here (1 sec) would not be enough to do positioning calculations. I miss the reversing;

I want to get the absolute X,Y position of the roomba to make maps of the cleaning it did. A lot of research led me to "forward kinematics" and "two wheeled robot odometry".

I started with some code I found at:

https://www.exceptionlife.com/robotics/question/10076/how-to-perform-odometry-on-an-arduino-for-a-differential-wheeled-robot

Which I modified into:

#define DISTANCEPERCOUNT 0.44432131 //in mm
#define WHEELDISTANCE 235.0 //in mm
#define ODOMETRYINTERVAL 100 //in ms
unsigned long p_odometryTimer = 0;
float theta = 0;
float Xcoo = 0;
float Ycoo = 0;
void Roomba632::UpdateOdometry(){
    if((millis() - p_odometryTimer)>ODOMETRYINTERVAL){
        p_odometryTimer = millis();
        static int encoderRPosPrev = 0;
        static int encoderLPosPrev = 0;
        float SR = DISTANCEPERCOUNT * (encoderRight - encoderRPosPrev);
        float SL = DISTANCEPERCOUNT * (encoderLeft - encoderLPosPrev);
        float meanDistance = (SL + SR)/2;
        encoderRPosPrev = encoderRight;
        encoderLPosPrev = encoderLeft;
        theta += (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);
    }
}

 This is starting to look promising!

I think I'm on the right track! (see what I did there) Whenever it bumps into something, it reverses a bit. I think this messes up the algorithm. Also I need a sensible sampling interval to calculate a realistic theta. There is much to solve here, but at least it seems possible. 

Next step is probably to capture a complete drive's worth of encoder data with a low interval. Then I want to do the coding in Octave so I can see the results to changes quicker and maybe I can "step-through" the data.

It was the power supply all allong! Never ignore a first hunch. 

More testing on the connection revealed packet losses when the roomba was docked. I wasn't expecting I could simply ping the ESP8266, but apparently you can. Whenever the roomba docked, ping times would increase and packets would be lost. 

This would ONLY happen when the roomba was docked and would be resolved immediately when it's removed from the dock.

So what's the difference between being docked and undocked? Power! It will get incoming serial communication in both scenario's. And all previous testing pointed to something other than software. I even put a hard ESP.restart(); in the onMqttDisconnect callback without proper result.

And providing the ESP with a antenna did help a bit. So it's time for something more substantial. Whenever in trouble or in doubt... add a big ol' C.

My hypothesis is that the raw battery voltage supplied by the roomba will fluctuate quite a bit when docked and hooked up to the charging circuitry. The buck step-down power converter needs some time to adjust and stabilise. On the other hand we have the ESP8266 with it's WiFi radio which is very sensitive to unstable power.

And one of the antennae that arrived in the post today. 

https://hackerstore.nl/Artikel/1026 

The black stick-on with quite a long wire so I have placement options:

And that's it! Boom! Stable as lead. I could immediately see better signal strength and bigger throughput on my router:

And monitoring the connection for a few hours made me quite confident. 

It's been running for a full 24 hours now, with 1 MQTT-reconnect. Over The Air updates also works stable now.

I hereby solemnly declare this problem resolved!

Guess I have to start on the hard bit and the math now ey?

short update 2022-04-10:

All this research and effort. Could it be solved by adding a WiFi antenna‽

My MQTT-LED-floorlamp will have to go without stable internet till I get some new antenna's in the mail. I had to scavenge around, because I didn't have the patience to wait for tomorrows mail delivery. 

It's also much easier to fumble around with some software than to disassemble the roomba so I kept stalling...

I was suspecting the hardware to be a source of my problems for a while now. I can imagine the electrical environment when docked and charging being quite noisy. I could also see the signal strength of the ESP8266 dropping on my routers info page, when docked. 

I could also see the MQTT connection being timed out on the mosquitto broker running on a Pi when reviewing the logs.

When I used some debug values in the code, I could see the WiFi connection would not drop completely and reconnect, but the MQTT-clients reconnect would fail with a socket error.

And the most important is that the MQTT connection is stable so the roomba can listen when it is time to clean. This still is it's primary function.

First few hours the problem seemed fixed. But then .. *sad trombone noises* .. The WiFi connection is more robust, but the MQTT connection still drops. Well, now I know what's NOT the problem. 

Next suspect on the list is the TCP-connection/MQTT-session. I now have the MQTT-broker running in -verbose mode for debugging. And all other connected appliances are down so I'm not being overwhelmed by all the info. To be continued.. once again.. 

Adding the antenna did not completely solve the wonky OTA though. It updates, but fails the check. This doesn't bother me for now because it behaves when I undock it. 

---

I've had connection issues for quite some time wich are VERY difficult to track down.

Symptoms:

• After some time, the MQTT-connection is lost. Reconnection will occur, but after this happens the first time the connection will be unstable;
• This only happens when the Roomba is docked;
• Whenever the connection is unstable, OTA also doesn't work;
• When Roomba is disconnected from the home base, it immediately reconnects and all is fine;
• When it reconnects, the serial receive buffer will be full;
• It's NOT my library/class (went back to a sketch with only MQTT and OTA);
• it's NOT the WiFi connection (switched network);

I discovered the PubSub MQTT reconnect function is blocking. So I switched libraries to a non-blocking one:

https://github.com/marvinroger/async-mqtt-client

It has a dependency which is not included:

https://github.com/me-no-dev/ESPAsyncTCP

Using the example, this made my sketch into:

#include <ESP8266WiFi.h>    //For ESP8266
#include <ESP8266mDNS.h>    //For OTA
#include <WiFiUdp.h>        //For OTA
#include <Ticker.h>         //For MQTT
#include <AsyncMqttClient.h>//For MQTT
#include <ArduinoOTA.h>     //For OTA
#include <ArduinoJson.h>
#include <Extras.h>
Extras SketchExtras;
#include <Roomba632.h>
Roomba632 roomba;

// WIFI configuration
#define WIFI_SSID "***"
#define WIFI_PASSWORD "***"

//OTA configuration
const char* host = "Roomba632"; //84ed23
//const char* host = "ESP8285_Testplatform"; //80e74a
const char * sketchpass = "**";

// MQTT configuration
#define MQTT_HOST IPAddress(192, 168, 1, 100)
#define MQTT_PORT 1883
#define MQTT_USER "***"
#define MQTT_PASSWORD "***"
#define MQTT_SUB_TOPIC "homeassistant/device/roomba/set"
#define MQTT_PUB_TOPIC "homeassistant/device/roomba/state"
#define MQTT_STREAM_TOPIC "homeassistant/device/roomba/stream"
#define MQTT_PUB_MESSAGE "MQTT-reconnected"
String mqtt_client_id = host;  //This text is concatenated with ChipId to get unique client_id
//unsigned long lastReconnectAttempt = 0;

unsigned long streamTimer = 0;
//#define STREAMINTERVAL 300000
#define STREAMINTERVAL 30000
//#define STREAMINTERVAL 1000

#define SERIAL_SIZE_RX  1024

AsyncMqttClient mqttClient;
Ticker mqttReconnectTimer;

WiFiEventHandler wifiConnectHandler;
WiFiEventHandler wifiDisconnectHandler;
Ticker wifiReconnectTimer;

//Connecting to Wi-Fi...
void connectToWifi() {
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
}

//Connecting to MQTT...
void connectToMqtt() {
  mqttClient.connect();
}

//Connected to Wi-Fi
void onWifiConnect(const WiFiEventStationModeGotIP& event) {
  connectToMqtt();
}

//Disconnected from Wi-Fi
void onWifiDisconnect(const WiFiEventStationModeDisconnected& event) {
  mqttReconnectTimer.detach(); // ensure we don't reconnect to MQTT while reconnecting to Wi-Fi
  wifiReconnectTimer.once(2, connectToWifi);
}

void onMqttConnect(bool sessionPresent) {
  mqttClient.subscribe(MQTT_SUB_TOPIC, 0);
  mqttClient.publish(MQTT_PUB_TOPIC, 0, true, "MQTT-Reconnected");
}

//Disconnected from MQTT
void onMqttDisconnect(AsyncMqttClientDisconnectReason reason) {
  if (WiFi.isConnected()) {
    mqttReconnectTimer.once(2, connectToMqtt);
  }
}

void onMqttSubscribe(uint16_t packetId, uint8_t qos) {}
void onMqttUnsubscribe(uint16_t packetId) {}

void onMqttMessage(char* topic, char* payload, AsyncMqttClientMessageProperties properties, size_t len, size_t index, size_t total) {
  char command = payload[0];
  // Switch case for commands only uses first char
  switch (command) {
    case 'C': { //Clean
      roomba.clean();
      break;
    }
    case 'D': { //Dock
      roomba.dock();
      break;
    }
    case 'S': { //Spot
      roomba.spot();
      break;
    }
    case 'X': { //Stop command
      roomba.stop();
      break;
    }
    default: 
      roomba.stop();
      break;
  }
}

void onMqttPublish(uint16_t packetId) {}

void setup() {
  //Serial.begin(115200);
  //Serial.setRxBufferSize(SERIAL_SIZE_RX);

  wifiConnectHandler = WiFi.onStationModeGotIP(onWifiConnect);
  wifiDisconnectHandler = WiFi.onStationModeDisconnected(onWifiDisconnect);

  ArduinoOTA.onStart([]() { //Make sure device is in a safe mode
    //store params to EEPROM?
  });
  ArduinoOTA.onEnd([]() { //Device will reboot

  });
  ArduinoOTA.onError([](ota_error_t error) {
    (void)error;
    ESP.restart();
  });
  ArduinoOTA.begin();
  mqttClient.onConnect(onMqttConnect);
  mqttClient.onDisconnect(onMqttDisconnect);
  mqttClient.onSubscribe(onMqttSubscribe);
  mqttClient.onUnsubscribe(onMqttUnsubscribe);
  mqttClient.onMessage(onMqttMessage);
  mqttClient.onPublish(onMqttPublish);
  mqttClient.setServer(MQTT_HOST, MQTT_PORT);
  mqttClient.setCredentials(MQTT_USER, MQTT_PASSWORD);

  connectToWifi();
  //mqttClient.publish(MQTT_PUB_TOPIC, 0, true, "ESP8266-Reset");
}

void loop() {
  ArduinoOTA.handle();
  roomba.handler();
  SketchExtras.Update();    
  if (millis() > streamTimer){
    streamTimer =  millis() + STREAMINTERVAL;
    char buffer[280];
    StaticJsonDocument<384> doc;
    doc["loops"] = SketchExtras.LoopsPerSec;
    doc["debug"] = roomba.debugVal;
    doc["data"] = roomba.dataState;
    doc["rS"] = roomba.roombaState;
    doc["dS"] = roomba.docked;
    doc["cS"] = roomba.chargingState;
    doc["bV"] = roomba.batteryVoltage;
    doc["bA"] = roomba.current;
    doc["bT"] = roomba.batteryTemp;
    doc["bC"] = roomba.batteryCharge;
    doc["bCap"] = roomba.batteryCapacity;
    doc["OI"] = roomba.OImode;
    doc["serA"] = Serial.available();
    doc["Song"] = roomba.songPlaying;
    doc["dD"] = roomba.dirtDetect;
    doc["mEL"] = roomba.encoderLeft;
    doc["mER"] = roomba.encoderRight;
    doc["rA"] = roomba.angle;
    doc["rD"] = roomba.distance;
    serializeJson(doc, buffer);
    mqttClient.publish(MQTT_STREAM_TOPIC, 0, true, buffer);
  } 
}

Aaaaaaand... it doesn't help :(

It's more efficient. So spending a few evenings wasn't entirely for nothing. Running at ~26000 main loops a second now instead of ~21000. But the reconnect issues are the same. AAArgh... 

To be continued...

The library / class has most of the functionality I want, but it's a bit of a mess. Time to do some rearranging, shuffling around of variables and functions. Throw away that one flag that's not been used for a while. Put all declarations back in place where they belong.

I decided to remove external control of the datastream. It's started and stopped when needed now :When it's not docked and receiving docked messages.

If its docked and there is a timeout on a correct docked message, it will start a datastream. When it is receiving the datastream and the bit for "docked" is set, datastream is stopped/paused and state will change to receiving the docked messages.

In states for cleaning/docking etc. it's monitored when the roomba has returned home and the state will change accordingly. This needs a bit of "debounce" time so it has time to actually leave the docking station. 

// Roomba632.cpp
// By Simon Jansen 2022-03-31 "the pandemic times"

#include "Roomba632.h"

#define DATASTREAMTIMEOUT 500
#define CHARGINGMESSAGETIMEOUT 3000
#define DATASTREAMMAXERROR 5
#define COMMANDSTARTTIMEOUT 50
#define COMMANDMODETIMEOUT 50
#define COMMANDTIMEOUT 20
#define DOCKEDDEBOUNCETIME 10000

Roomba632::Roomba632(){
    //Publicaly set variables
    //Privately set public variables
    chargingState = chargingStateNotCharging;
    batteryVoltage = 0;
    current = 0;
    batteryTemp = 0;
    batteryCharge = 0;
    batteryCapacity = 0;
    docked = false;
    OImode = 0; //enum for states
    encoderLeft = 0;
    encoderRight = 0;
    angle = 0; //calculated from encoders
    distance = 0; //calculated from encoders

    //Privately set public states. Only set and change states from within state machine.
    roombaState = roombaIdle; //enum for states
    dataState = dataIdle;
    p_undockedTimer = millis();
    //Privately set public flags
    b_songPlaying = false;
    b_dirtDetected = false;
    //Private flags and variables
    p_cleanFlag = false;
    p_dockFlag = false;
    p_spotFlag = false;
    p_stopFlag = false;
    p_playMusicFlag = false;
    p_driveFlag = false;

    //Private counters and timers
    p_musicCounter = 0; //could be static?
    p_musicTimer = 0; //could be static?

    //Initialize serial connection to Roomba
    Serial.begin(115200);
}

//public functions for change in state set a flag (and vairables) as a 'request' whitch is handled in order of prirority
void Roomba632::stop(){
    SendStopCommand();
    p_stopFlag = true;
}
void Roomba632::clean(){
    p_cleanFlag = true;
}
void Roomba632::dock(){
    p_dockFlag = true;
}

void Roomba632::handler(){
    //state machine for high level states
    switch (roombaState){
        case roombaHome: //Charging
            //Check flags in appropriate order of prirority
            if(p_cleanFlag){
                SendCleanCommand();
            }
        break;
        case roombaIdle: //IDLE: not charging, driving, playing music or cleaning
            p_stopFlag = false; //Clear stop request flag if set
            //Keep alive
            //Check flags in appropriate order of prirority
            if(p_cleanFlag){
                SendCleanCommand();
            }
            else if(p_spotFlag){
                SendSpotCommand();
            }
            else if(p_dockFlag){
                SendDockCommand();
            }
            else if(p_playMusicFlag){
                SendPlayMusicCommand();
            }    
            if(millis() > p_undockedTimer){
                if(docked){
                    roombaState = roombaHome;
                }
            }
            break;
        case roombaCleaning:
            p_cleanFlag = false; //clear request flag
            if(p_stopFlag){
                SendStopCommand();
            }
            else if(p_dockFlag){
                SendDockCommand();
            }
            else if(p_spotFlag){
                SendSpotCommand();
            }
            else if(p_playMusicFlag){
                SendPlayMusicCommand();
            }
            if(millis() > p_undockedTimer){
                if(docked){
                    roombaState = roombaHome;
                }
            }
            break;
        case roombaSpotCleaning:
            p_spotFlag = false; //clear request flag
            if(p_stopFlag){
                SendStopCommand();
            }
            else if(p_cleanFlag){
                SendCleanCommand();
            }
            else if(p_dockFlag){
                SendDockCommand();
            }
            else if(p_playMusicFlag){
                SendPlayMusicCommand();
            }    
            if(millis() > p_undockedTimer){
                if(docked){
                    roombaState = roombaHome;
                }
            }
            break;
        case roombaDocking:
            p_dockFlag = false; //clear request flag
            if(p_stopFlag){
                SendStopCommand();
            }
            else if(p_cleanFlag){
                SendCleanCommand();
            }
            else if(p_spotFlag){
                SendSpotCommand();
            }
            else if(p_playMusicFlag){
                SendPlayMusicCommand();
            }    
            if(millis() > p_undockedTimer){
                if(docked){
                    roombaState = roombaHome;
                }
            }
            break;
        case roombaDriving:
            p_driveFlag = false;
            roombaState = roombaIdle;
            break;
        case roombaMusic:
            p_playMusicFlag = false;
            //time music or poll state
            roombaState = roombaIdle;
            break;
    }
    //State machine for getting data from Roomba in docked and undocked state.
    //
    static unsigned long messagetimeouttimer, commandTimer;
    static char* pos;
    static char buffer[7];
    static char i, j, errorCounter;
    static char chargingHeaderBuffer[10], chargingMessageBuffer[80];
    static byte databuffer[30]; // 12 packetID's + 18 databytes
    static byte checksum;
    
    switch (dataState){
        //Idle: try to start datastream
        case dataIdle:
            //try to start datastream
            dataState = datastreamStart;
        break;
        //Handle serial charging message
        //Example:
        //"bat:   min 16  sec 35  mV 16198  mA 1102  tenths-deg-C 223  mAH 2496  state 5"
        case chargingMessageWaitingForHeader:
            if(Serial.available()){
                if(Serial.read() =='b'){
                    dataState = chargingMessageReadingHeader;
                    //p_requestStopDatastreamFlag = true;
                    j=0;
                }
            }
            //timeout?
            else if (millis() > messagetimeouttimer){
                   //start datastream to figure out what it's up to
                dataState = datastreamStart;
            }
        break;
        case chargingMessageReadingHeader:
            if (Serial.available()){
                chargingHeaderBuffer[j] = Serial.read();
                j++;
                if (j==10){
                    if (strncmp(chargingHeaderBuffer, "at:   min ",10)==0){ 
                        dataState = chargingMessageReadingMessage;
                        messagetimeouttimer = millis() + CHARGINGMESSAGETIMEOUT; //reset timer
                    }
                    else{
                        dataState = chargingMessageWaitingForHeader;
                    }
                    j = 0;
                }
            }
            //timeout?
            else if (millis() > messagetimeouttimer){
                   //start datastream to figure out what it's up to
                dataState = datastreamStart;
            }
            break;
        case chargingMessageReadingMessage:
            if(Serial.available()){
                chargingMessageBuffer[j] = Serial.read();
                if (j==79){
                    j=0;
                    dataState = chargingMessageReadingHeader;
                }
                if (chargingMessageBuffer[j] == '\n'){
                    pos = strtok(chargingMessageBuffer, " ");
                    j = 1;                
                    while (pos != NULL){
                        if (j== 5){ //Voltage
                            strcpy(buffer, pos);
                            batteryVoltage = atol(buffer);
                        }
                        if (j== 7){
                            strcpy(buffer, pos);
                            current = atol(buffer);
                        }
                        if (j== 9){
                            strcpy(buffer, pos);
                            batteryTemp = atol(buffer)/10; //Gaat nog niet goed. Is in tienden! Dus delen door 10
                        }
                        if (j== 11){
                            strcpy(buffer, pos);
                            batteryCapacity = atol(buffer);
                        }
                        if (j== 13){
                            strcpy(buffer, pos);
                            chargingState = atoi(buffer);
                        }
                        j++;
                        pos = strtok(NULL, " ");
                    }
                    //state = roombaHome;
                    //charging = true;
                    //docked = true;
                    //p_requestStopDatastreamFlag = true;                
                    dataState = chargingMessageReadingHeader;
                    j = 0;
                }
                j++;
            }
            //timeout?
            else if (millis() > messagetimeouttimer){
                   //start datastream to figure out what it's up to
                dataState = datastreamStart;
            }
        break;
        //Handle datastream
        case datastreamStart:
            Serial.write(OCstart); //Roomba will change to passive mode
            //maybe clear data?
            commandTimer = millis() + COMMANDSTARTTIMEOUT;        
            dataState = datastreamStarting;
        break;
         case datastreamStarting:
            if (millis() > commandTimer){
                //setup datastream
                Serial.write(OCDatastream); //start stream
                Serial.write(12); //12 packets
                Serial.write(15); //1- dirt detect U 1
                Serial.write(21); //2- Charging state U 1
                Serial.write(22); //3- voltage U 2
                Serial.write(23); //4- current S 2
                Serial.write(24); //5- Battery temp S 1
                Serial.write(25); //6- Battery charge U 2
                Serial.write(26); //7- Battery capacity U 2
                Serial.write(34); //8- Charging sources available U 1
                Serial.write(35); //9- OI mode U 1
                Serial.write(37); //10- Song playing? U 1
                Serial.write(43); //11- Wheel encoder counts left S 2
                Serial.write(44); //12-Wheel encoder counts right S 2
                //serial write startDatastream
                dataState = datastreamWaitingForHeader;
                //Set timer
                messagetimeouttimer = millis() + DATASTREAMTIMEOUT;
            }
            break;
        case datastreamWaitingForHeader:
            //disentangle datastream and do checksum for valid data. 
            if (Serial.available()){
                byte serialByte= Serial.read();
                if(serialByte == 19){ //Header for datastream (save to variable to do multiple checks for charging message etc)
                    dataState = datastreamWaitingForNbytes;
                    messagetimeouttimer = millis() + DATASTREAMTIMEOUT; //reset timer
                }            
                //timeout
                else if (millis() > messagetimeouttimer){
                    messagetimeouttimer = millis() + DATASTREAMTIMEOUT; //reset timer
                    errorCounter++;
                    if (errorCounter == DATASTREAMMAXERROR){
                        dataState = datastreamStopping;
                    }
                }
            }
            break;    
        case datastreamWaitingForNbytes:
            if (Serial.available()){
                if(Serial.read() == 30){ //number of bytes
                    dataState = datastreamReadingData;
                    i = 0;
                    checksum = 49; //19 + 30 starting value of checksum
                    messagetimeouttimer = millis() + DATASTREAMTIMEOUT; //reset timer
                }    
                else { //number of bytes should imediately follow header. Else, header was just a rando 19 value
                    //Go back to finding the next header
                    dataState = datastreamWaitingForHeader;
                }
            }    
            //timeout
            else if (millis() > messagetimeouttimer){
                messagetimeouttimer = millis() + DATASTREAMTIMEOUT; //reset timer
                errorCounter++;
                if (errorCounter == DATASTREAMMAXERROR){
                    dataState = datastreamStopping;
                }
            }
            break;
        case datastreamReadingData:
            if(Serial.available()){
                databuffer[i] = Serial.read();
                checksum += databuffer[i];
                if(i==29){
                    dataState = datastreamCheckingData;
                    messagetimeouttimer = millis() + DATASTREAMTIMEOUT; //Reset timer
                }
                i++;
            }
            //timeout
            else if (millis() > messagetimeouttimer){
                messagetimeouttimer = millis() + DATASTREAMTIMEOUT; //reset timer
                errorCounter++;
                if (errorCounter == DATASTREAMMAXERROR){
                     dataState = datastreamStopping;
               }
            }
            break;
        case datastreamCheckingData:
            if(Serial.available()){
                checksum += Serial.read(); //Addition to checksum should give 256 on rollover of byte 0
                if (checksum == 0){
                    //checksum passed
                    dirtDetect = databuffer[1];
                    chargingState = databuffer[3];
                    batteryVoltage = (databuffer[5] << 8) | databuffer[6];
                    current = (databuffer[8] << 8) | databuffer[9];
                    batteryTemp = databuffer[11];
                    batteryCharge = (databuffer[13] << 8) | databuffer[14];
                    batteryCapacity = (databuffer[16] << 8) | databuffer[17];
                    docked = bitRead(databuffer[19],1);
                    OImode = databuffer[21];
                    songPlaying = databuffer[23];
                    encoderLeft = (databuffer[25] << 8) | databuffer[26];
                    encoderRight = (databuffer[28] << 8) | databuffer[29];
                    //dataReady = true;
                    errorCounter = 0;
                    if(docked){

                        dataState = datastreamStopping;
                    }
                    else{
                        dataState = datastreamWaitingForHeader;
                    }
                }
                else{
                    //checksum failed
                    errorCounter++;
                    if (errorCounter == DATASTREAMMAXERROR){
                         dataState = datastreamStopping;
                   }
                }
            }
            //timeout
            else if (millis() > messagetimeouttimer){
                dataState = datastreamWaitingForHeader;
                errorCounter++;
                if (errorCounter == DATASTREAMMAXERROR){
                     dataState = datastreamStopping;
                }
            }
            break;
        case datastreamStopping:
            //handle last message
            //serial write stop datastream
            Serial.write(OCPauseDatastream);
            Serial.write(0);
            Serial.write(OCStop);
             dataState = chargingMessageWaitingForHeader;
            messagetimeouttimer = millis() + CHARGINGMESSAGETIMEOUT; //reset timer
            errorCounter = 0; 
            break;
     }
}

void Roomba632::SendStopCommand(){
    static bool commandstate;
    static unsigned long commandTimer;
    if (!commandstate){
        Serial.write(OCStop);
        commandstate = true;
        commandTimer = millis() + COMMANDSTARTTIMEOUT;
    }
    else{        
        if (millis() > commandTimer){
            commandstate = false;
            roombaState = roombaIdle;            
        }
    }
}
void Roomba632::SendCleanCommand(){
    static int commandstate;
    static unsigned long commandTimer;
    switch (commandstate){
        case startIO:
            Serial.write(OCstart);
            commandstate = setMode;
            commandTimer = millis() + COMMANDSTARTTIMEOUT;
            break;
        case setMode:
            if (millis() > commandTimer){
                Serial.write(OCSafemode);
                commandstate = sendCommand;
                commandTimer = millis() + COMMANDMODETIMEOUT;
            }
            break;
        case sendCommand:
            if (millis() > commandTimer){
                Serial.write(OCClean);
                commandstate = startIO; //reset state
                roombaState = roombaCleaning;
                p_undockedTimer = millis()+DOCKEDDEBOUNCETIME;
            }
            break;                    
    }
}
void Roomba632::SendDockCommand(){
    static int commandstate;
    static unsigned long commandTimer;
    switch (commandstate){
        case startIO:
            Serial.write(OCstart);
            commandstate = setMode;
            commandTimer = millis() + COMMANDSTARTTIMEOUT;
            break;
        case setMode:
            if (millis() > commandTimer){
                Serial.write(OCSafemode);
                commandstate = sendCommand;
                commandTimer = millis() + COMMANDMODETIMEOUT;
            }
            break;
        case sendCommand:
            if (millis() > commandTimer){
                Serial.write(OCDock);
                commandstate = startIO; //reset state
                roombaState = roombaDocking;
                p_undockedTimer = millis()+DOCKEDDEBOUNCETIME;
            }
            break;                    
    }
}

Previously, I had trouble keeping the (MQTT-) connection stable. This would only happen when the roomba was docked. And when the connection returned it would always have a full Serial.available() buffer. I suspected this had something to do with interrupts and the incoming serial buffer. I tried to use a bigger Serial Rx buffer, but this didn't help. It looks to be stable now, so I'm hoping it's sorted.

Neat! Now I can finally begin with positioning data :)

---

-2022-04-02; Small update: Uncomment the line

//Serial.write(OCStop);

in the datastreamStopping case. Otherwise there will be no charging message, so it will timeout - start datastream - docked==true - stop datastream - repeat. 

Updated header and source files are uploaded.

I'm still having stability / connection issues though and I don't think its directly related to the interpretation of the incoming message. I disabled all interpretation and just clear the incoming buffer with a Serial.read(); statement. This didn't help. 

In the main sketch I'm using https://arduinojson.org/ to construct my MQTT-message. This is quite strict on memory size declarations and I hadn't updated this with some changes I made. When I fixed this it seemed a permanent fix until it didn't.

It does have something to do with being docked. Whenever it's "stuck", I can just remove it from its base and it will reconnect immediately. Could it be a hardware issue? Or something obscure in the way the ESP8266 handles incoming Serial comms?

The code is starting to come together. I wanted a way to check when the roomba is docked and charging. Of course I could use the datastream for this. It has a bit set for when it's docked at its home base and a byte for its charging state.

But when it's docked, the roomba also gives a message every second. Something like:

bat:   min 16  sec 35  mV 16198  mA 1102  tenths-deg-C 223  mAH 2496  state 5

 When it's docked, I don't need the high speed datastream. So I want to switch over to getting the info from this message instead. Then the ESP8266 has its processing power freed up to do ... Idunno ¯\_(ツ)_/¯

The docked message is in ASCII instead of bytes. I want the values to be interpreted AND to be throughput to MQTT (when debugging). 

You know what this means of course: 

IT'S HEADACHE-... I MEAN CHARACTER-ARRAY-TIME!

Some of the characteristics of the message we're dealing with:

• The message is terminated with a line feed char '\n';
• It always starts the same "bat:   min ....";
• It has a fixed number of elements;
• The elements are in a fixed order;
• Every element has a name and a value;
• Name and value are separated by one space;
• All elements are separated by two spaces;
• All values are represented by integers;
• The element-headers only appear in this message and in no other message (such as boot up);

I messed around with a few different ways to interpret the message. 

• Get the complete message in a shifting buffer (or circular buffer with rotating index). Then await the correct header, set the starting index and interpret the complete message;
• Ignore any message header and focus on the elements. Do all checks parallel. So pass the incoming character to multiple functions and only catch the separate elements;

In the end I settled on the following: 

• Awaiting the first character of the header 'b'. This is a simple check so won't gobble up processing time for every incoming character;
• When the first character of the header is found. Try checking for the rest of the header message: "at:  min ". If this condition is met, we can be confident we found the start of a docked message;
• Put the rest of the incoming characters in a buffer until the end of the message (or a timeout and throw away the message);
• Split the buffered message in separate elements and interpret the values;

All is done in a separate function that gets passed an incoming character. It uses C string functions as strcmp(), strcpy() and strtok(). 

void Roomba632::CheckChargingMessage(char incommingChar){
    //Example:
    //"bat:   min 16  sec 35  mV 16198  mA 1102  tenths-deg-C 223  mAH 2496  state 5"
    static int chargingMessageState;
    char* pos;
    char buffer[7];
    static char j;
    static char chargingHeaderBuffer[10], chargingMessageBuffer[80];
    switch (chargingMessageState){
        case chargingMessageWaitingForHeader:
            if(incommingChar =='b'){
                chargingMessageState = chargingMessageReadingHeader;
                p_requestStopDatastreamFlag = true;
                j=0;
            }
        break;
        case chargingMessageReadingHeader:
            chargingHeaderBuffer[j] = incommingChar;
            j++;
            if (j==10){
                if (strncmp(chargingHeaderBuffer, "at:   min ",10)==0){ 
                    chargingMessageState = chargingMessageReadingMessage;
                }
                else{
                    chargingMessageState = chargingMessageWaitingForHeader;
                }
                j = 0;
            }
        break;
        case chargingMessageReadingMessage:
            chargingMessageBuffer[j] = incommingChar;
            j++;
            if (j==79){
                j=0;
                chargingMessageState = chargingMessageReadingHeader;
            }
            if (incommingChar == '\n'){
                pos = strtok(chargingMessageBuffer, " ");
                j = 1;
                
                while (pos != NULL){
                    if (j== 5){ //Voltage
                        strcpy(buffer, pos);
                        batteryVoltage = atol(buffer);
                    }
                    if (j== 7){
                        strcpy(buffer, pos);
                        current = atol(buffer);
                    }
                    if (j== 9){
                        strcpy(buffer, pos);
                        batteryTemp = atol(buffer)/10; //Decimals are dropped
                    }
                    if (j== 11){
                        strcpy(buffer, pos);
                        batteryCapacity = atol(buffer);
                    }
                    if (j== 13){
                        strcpy(buffer, pos);
                        chargingState = atoi(buffer);
                    }
                    j++;
                    pos = strtok(NULL, " ");
                }
                state = roombaHome;
                charging = true;
                docked = true;
                p_requestStopDatastreamFlag = true;                
                chargingMessageState = chargingMessageReadingHeader;
                j = 0;
            }
        break;
    }
}

 The states are all defined with a bunch of enumerations in the header file. The code is far from perfect, but it works quite nicely and is another element for my roomba class. 

Still some improvements: I'm setting states of other state machines outside of the state machines. This is a big no-no that I will try to solve when I tidy up everything.

Also, I'm quite charmed by the idea of a circular buffer with a rotating index and might try that too.

For now, it'll do. I'm getting ~23000 main loops every second. So maybe it has enough spare processing power to do my taxes or plot for world domination?? 

I'm getting confident that my plans could actually work. Software is getting there. I'm getting inputs/sensor readings without delays so the ESP8266 can do some real processing soon. I love it when a plan comes together! 

Time to sort out the power situation and add the IMU-hardware.

When I opened up the roomba I could see the LM7833 was indeed getting quite hot. The packing I added so everything was snug had actually melted/deformed a bit. The board also had some discoloration.

I still have plenty of space to change out the LM7833 with the Buck DC/DC step down power converter. This is the one I'm using: https://www.tinytronics.nl/shop/nl/power/spanningsconverters/buck-(step-down)-converters/dc-dc-step-down-buck-converter-1.5a-3.3v-output

Desolder the LM7833, some rearranging with the wires, connect the GND's together and it all fits.

For the IMU, I'm using this:

https://www.tinytronics.nl/shop/nl/sensoren/acceleratie-rotatie/mpu-6050-accelerometer-en-gyroscope-3-axis-module-3.3v-5v

I want the IMU as close to the center of rotation as possible. This is in the middle between the wheels and turns out to be the location of the "clean" button. I will pace it just trailing the center when driving. I think this will give me the least problems when calculation position / track. Approximate location:

The panel for the buttons has some room underneath where I can also run the required wires.

For the MPU6050, I need 5 connections:

VDD, GND, I2C-Clock, I2C-Data, Data ready interrupt. The address pin is connected directly to GND on the board.

The clock gets a 5k pull-up to VDD and is connected to GPIO14 of the ESP8266;

Data needs a pull-up to VDD. I connect this to GPIO02 of the ESP8266 which already has a 10k pull-up to set the boot mode;

Interrupt needs a pull-down to GND (interrupt is on rising edge). So I connected this to GPIO15 which already has a 10k pull-down to set the boot mode;

IMU and Buck converter all connected:

The IMU is fixed in place with some double sided foam tape. Neat!:

And everything in it's place, hopefully never to be seen again:

Closing everything up. Crossing fingers, safety glasses on and place the battery... 

We're stil up and running! I have it set up so that it publishes it's internal values every 5 seconds when I enable the datastream.

The datastream will also continue 24/7 and is not interrupted by cleaning or other button presses.

So the ESP8266 get's the motor encoder data 66 times a second and IMU-data (from the DMP) 100 times a second? 

It's getting exciting now!