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:
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
Become a Hackaday.io Member
Create an account to leave a comment. Already have an account? Log In.