Close
0%
0%

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.

Public Chat
Similar projects worth following
Using an ESP8266 module, control my Roomba 632 that I bought on Ibood some years ago.
It will clean my house, won't look like something I hacked together and is a perfect platform for experimentation.

Goals:
X - Add control with MQTT in Home Assistant for scheduled cleaning;
X - Play Macgyver theme song;
X - Read sensorstream data;
X - Add IMU;
X - Clean code;
X - Get vectordata from wheel encoder readings for location;
X - Correct vectordata with closed loop;
X - Integrate IMU and wheel encoder readings to improve accuracy;
O - Mapping;
O - Controll and SLAM?
X - Having fun ;-)

Ok. This project will take a while... A lot of stuff coming together on this one. 

So I found out that the Roomba I've had for some years has a serial port and an open interface.

It's basically a iRobot Create 2, but this one can also clean my floor:

https://edu.irobot.com/what-we-offer/create-robot 

The Roomba is a 632 (600 series) without any scheduling or mapping capabilities... for now.

The interface allows me to not only send basic commands, but also to get full sensor readings. On everything from motors, bump and cliff sensors etc. 

As always, the plan is pretty simple. Add microcontroller, some soldering, some typing, done!

To start, I want to be able to let Home Assistant send a CLEAN command via MQTT on a schedule.

In the long run I want to add a IMU (MPU6050) and integrate this with the sensordata (odometry) from the Roomba to get mapping data. So tracks like you get with a handheld GPS.

Then maybe do some loop-closing geodesy magic and take full control of the driving and cleaning.

ino - 12.54 kB - 04/23/2022 at 21:11

Download

async-mqtt-client-develop.zip

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

Zip Archive - 52.10 kB - 04/06/2022 at 20:32

Download

ESPAsyncTCP-master.zip

Dependency for the Async MQTT-client https://github.com/me-no-dev/ESPAsyncTCP

Zip Archive - 51.53 kB - 04/06/2022 at 20:32

Download

Roomba632.h

Library header containing the Roomba632 class v.2022-04-23

text/plain - 4.87 kB - 02/03/2022 at 18:23

Download

Roomba632.cpp

Library source containing the Roomba632 class v.2022-04-23

text/plain - 22.86 kB - 02/03/2022 at 18:22

Download

View all 10 files

  • 1 × iRobot Roomba 632
  • 1 × ESP8266
  • 1 × IPEX antenna WiFi antenna for the ESP8266: https://hackerstore.nl/Artikel/1026
  • 1 × Buck step down DC/DC converter 3.3V
  • 1 × MPU6050

  • Properly implementing the IMU

    Simon Jansen04/23/2022 at 21:08 0 comments

    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. 

  • Calibrating the system

    Simon Jansen04/19/2022 at 21:00 0 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){
    ...
    Read more »

  • It's all iterative

    Simon Jansen04/17/2022 at 15:49 0 comments

    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 :)

  • Tracking algorithm

    Simon Jansen04/16/2022 at 14:13 0 comments

    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.

  • Roomba's first steps... -recording algorithm

    Simon Jansen04/15/2022 at 09:27 0 comments

    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.

  • Connection issues fixed! 4 realz

    Simon Jansen04/10/2022 at 11:05 0 comments

    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?

  • Fixing connection issues

    Simon Jansen04/06/2022 at 20:17 0 comments

    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...
    Read more »

  • Spring Cleaning

    Simon Jansen03/30/2022 at 13:54 0 comments

    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...
    Read more »

  • Mooooor code

    Simon Jansen03/15/2022 at 08:37 0 comments

    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(...
    Read more »

  • Power to the roomba!

    Simon Jansen02/06/2022 at 11:37 0 comments

    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! 

View all 18 project logs

Enjoy this project?

Share

Discussions

Similar Projects

Does this project spark your interest?

Become a member to follow this project and never miss any updates