Close

Roomba's first steps... -recording algorithm

A project log for Old Roomba, new tricks

Add mapping & MQTT-interfacing with ESP8266 and an IMU

simon-jansenSimon Jansen 04/15/2022 at 09:270 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:


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.

Discussions