Well! I learned a lot these days :)
The first version of the library / class seems so work and seems stable. It's still a work in progress. I will upload a zip if it's nearer to complete.
For now it can put the roomba in diferent states by sending commands without delays. It can request and interpret a datastream (with checksum!) and make the data accesable for further processing.
All "commands" are received as "requests" and handled according to set priority in state machines. The states are strictly controlled. For instance: When "cleaning" it's useless to send another cleaning command.
Header:
#ifndef ROOMBA632_H
#define ROOMBA632_H
// Arduino versioning.
#include "Arduino.h"
enum OImodes {
Off = 0,
Passive = 1,
Safe = 2,
Full = 3
};
enum states {
roombaIdle,
roombaCharging,
roombaCleaning,
roombaSpotCleaning,
roombaDocking,
roombaDriving,
roombaMusic
};
enum receivedDatastreamStates {
datastreamIdle,
datastreamStarting,
datastreamWaitingForHeader,
datastreamWaitingForNbytes,
datastreamReadingData,
datastreamCheckingData,
datastreamStopping,
};
enum sendingCommandStates {
sendingCommandsIdle,
sendingCommandsSetMode,
sendingCommandsSending,
};
enum commandStates{
startIO,
setMode,
sendCommand
};
enum opcodes{
OCstart = 128,
OCSafemode = 131,
OCFullmode = 132,
OCSpot = 134,
OCClean = 135,
OCDrive = 137,
OCLeds = 139,
OCSong = 140,
OCPlaySong = 141,
OCDatastream = 148,
OCPauseDatastream = 150,
OCDock = 143,
OCStop = 173
};
class Roomba632 {
public:
Roomba632();
void handler();
//Set a flag (and variables) as a "request" witch is handled in order of priority:
void stop();
void clean();
void spot();
void dock();
void startDatastream();
void stopDatastream();
void playMusic();
void drive();
//Privately set public variables
uint8_t dirtDetect;
uint16_t batteryVoltage;
int16_t current;
int8_t batteryTemp;
uint16_t batteryCharge;
uint16_t batteryCapacity;
uint8_t OImode; //enum for states
uint8_t songPlaying;
int16_t encoderLeft;
int16_t encoderRight;
int16_t angle; //calculated from encoders
int8_t distance; //calculated from encoders
//Privately set public states. Only set and change states from within state machine.
int state; //enum for states
//int receivingDatastreamState = datastreamIdle; //enum for states
//int sendingCommands = sendingCommandsIdle; //enum for states
//Privately set public flags
volatile bool dataReady;
volatile bool charging;
volatile bool b_songPlaying;
volatile bool b_dirtDetected;
volatile bool sensorTripped; //for bump cliff and wheel drop sensors
private:
//unsigned long _songLength(song as argument); //song
void SendStopCommand();
void SendCleanCommand();
void SendDockCommand();
void SendSpotCommand();
void SendPlayMusicCommand();
void _evilLights();
//Private flags
volatile bool p_cleanFlag;
volatile bool p_dockFlag;
volatile bool p_spotFlag;
volatile bool p_stopFlag;
volatile bool p_requestStopDatastreamFlag;
volatile bool p_requestStartDatastreamFlag;
volatile bool p_playMusicFlag;
volatile bool p_driveFlag;
//Private counters and timers
uint8_t p_musicCounter; //could be static?
unsigned long p_musicTimer; //could be static?
};
#endif
And source:
// Roomba632.cpp
// By Simon Jansen 2021-01-29 "the pandemic times"
#include "Roomba632.h"
#define DATASTREAMTIMEOUT 1000
#define DATASTREAMMAXERROR 3
#define COMMANDSTARTTIMEOUT 20
#define COMMANDMODETIMEOUT 20
#define COMMANDTIMEOUT 20
Roomba632::Roomba632(){
//Publicaly set variables
//music song to be played
//driving instructions
//Privately set public variables
batteryVoltage = 0;
current = 0;
batteryTemp = 0;
batteryCharge = 0;
batteryCapacity = 0;
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.
state = roombaIdle; //enum for states
//p_requestStartDatastreamFlag = false;
//int receivingDatastreamState = datastreamIdle; //enum for states
//int sendingCommands = sendingCommandsIdle; //enum for states
//Privately set public flags
dataReady = false;
charging = false;
b_songPlaying = false;
b_dirtDetected = false;
sensorTripped = false; //for bump cliff and wheel drop sensors
//Private flags
p_cleanFlag = false;
p_dockFlag = false;
p_spotFlag = false;
p_stopFlag = false;
p_requestStartDatastreamFlag = false;
p_requestStopDatastreamFlag = 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(){
p_stopFlag = true;
}
void Roomba632::clean(){
p_cleanFlag = true;
}
void Roomba632::spot(){
p_spotFlag = true;
}
void Roomba632::dock(){
p_dockFlag = true;
}
void Roomba632::startDatastream(){
p_requestStartDatastreamFlag = true;
}
void Roomba632::stopDatastream(){
p_requestStopDatastreamFlag = true;
}
/*
void Roomba632::playMusic(arguments){
//set music to be played
p_playMusicFlag = true;
}
void Roomba632::drive(arguments){
//set driving variables
p_driveFlag = true;
}
*/
void Roomba632::handler(){
//state machine for high level states
switch (state){
case roombaCharging: //Charging
//Check charging flag
if (!charging){ //Roomba wandered off!
//start datastream to figure out what it's up to
}
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();
}
break;
/*
else if(p_driveFlag){
Serial.write(OCstart);
Serial.write(OCSafemode);
Serial.write(OCSpot);
state = roombaCleaning;
}
*/
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();
}
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();
}
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();
}
break;
case roombaDriving:
p_driveFlag = false;
state = roombaIdle;
break;
case roombaMusic:
p_playMusicFlag = false;
//time music or poll state
state = roombaIdle;
break;
}
//state machine for handling datastream
static int datastreamState; //should be initialized to 0 thus Idle state wich is also default
static unsigned long commandTimer;
static unsigned long timeouttimer;
static byte databuffer[26];
static char i, errorCounter;
static byte checksum;
switch (datastreamState){
case datastreamIdle: //datastreamIdle, 0, IDLE
p_requestStopDatastreamFlag = false;
if (p_requestStartDatastreamFlag){
Serial.write(OCstart); //Roomba will change to passive mode
//maybe clear data?
datastreamState = datastreamStarting;
commandTimer = millis() + COMMANDSTARTTIMEOUT;
}
//Check for charging message
break;
case datastreamStarting:
if (millis() > commandTimer){
//setup datastream
Serial.write(OCSafemode);
Serial.write(OCDatastream); //start stream
Serial.write(10); //10 packets
Serial.write(15); //1- dirt detect U 1
Serial.write(22); //2- voltage U 2
Serial.write(23); //3- current S 2
Serial.write(24); //4- Battery temp S 1
Serial.write(25); //5- Battery charge U 2
Serial.write(26); //6- Battery capacity U 2
Serial.write(35); //7- OI mode U 1
Serial.write(37); //8- Song playing? U 1
Serial.write(43); //9- Wheel encoder counts left S 2
Serial.write(44); //10-Wheel encoder counts right S 2
//serial write startDatastream
datastreamState = datastreamWaitingForHeader;
//Set timer
timeouttimer = millis() + DATASTREAMTIMEOUT;
}
break;
case datastreamWaitingForHeader:
p_requestStartDatastreamFlag = false;
if (p_requestStopDatastreamFlag){
datastreamState = datastreamStopping;
}
//check for charging message
//set charging flag
//charging = true;
//reset charging flag
//charging = false;
//disentangle datastream and do checksum for valid data.
if (Serial.available()){
if(Serial.read()==19){ //Header for datastream
datastreamState = datastreamWaitingForNbytes;
timeouttimer = millis() + DATASTREAMTIMEOUT; //reset timer
}
//timeout
else if (millis() > timeouttimer){
datastreamState = datastreamIdle;
errorCounter++;
if (errorCounter == DATASTREAMMAXERROR){
p_requestStopDatastreamFlag = true;
datastreamState = datastreamStopping;
errorCounter = 0;
}
}
}
break;
case datastreamWaitingForNbytes:
if (Serial.available()){
if(Serial.read()== 26){ //number of bytes
datastreamState = datastreamReadingData;
i = 0;
checksum = 45; //19 + 26
timeouttimer = millis() + DATASTREAMTIMEOUT; //reset timer
}
else{
errorCounter++;
if (errorCounter == DATASTREAMMAXERROR){
p_requestStopDatastreamFlag = true;
datastreamState = datastreamStopping;
errorCounter = 0;
}
}
}
//timeout
if (millis() > timeouttimer){
datastreamState = datastreamIdle;
errorCounter++;
if (errorCounter == DATASTREAMMAXERROR){
p_requestStopDatastreamFlag = true;
datastreamState = datastreamStopping;
errorCounter = 0;
}
}
break;
case datastreamReadingData:
if(Serial.available()){
databuffer[i] = Serial.read();
checksum += databuffer[i];
if(i==25){
datastreamState = datastreamCheckingData;
timeouttimer = millis() + DATASTREAMTIMEOUT; //Reset timer
}
i++;
}
//timeout
else if (millis() > timeouttimer){
datastreamState = datastreamIdle;
errorCounter++;
if (errorCounter == DATASTREAMMAXERROR){
p_requestStopDatastreamFlag = true;
datastreamState = datastreamStopping;
errorCounter = 0;
}
}
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];
batteryVoltage = (databuffer[3] << 8) | databuffer[4];
current = (databuffer[6] << 8) | databuffer[7];
batteryTemp = databuffer[9];
batteryCharge = (databuffer[11] << 8) | databuffer[12];
batteryCapacity = (databuffer[14] << 8) | databuffer[15];
OImode = databuffer[17];
songPlaying = databuffer[19];
encoderLeft = (databuffer[21] << 8) | databuffer[22];
encoderRight = (databuffer[24] << 8) | databuffer[25];
dataReady = true;
}
else{
//checksum failed
errorCounter++;
if (errorCounter == DATASTREAMMAXERROR){
p_requestStopDatastreamFlag = true;
datastreamState = datastreamStopping;
errorCounter = 0;
}
}
datastreamState = datastreamWaitingForHeader;
}
//timeout
else if (millis() > timeouttimer){
datastreamState = datastreamIdle;
errorCounter++;
if (errorCounter == DATASTREAMMAXERROR){
p_requestStopDatastreamFlag = true;
datastreamState = datastreamStopping;
errorCounter = 0;
}
}
break;
case datastreamStopping:
//handle last message
//serial write stop datastream
Serial.write(OCPauseDatastream);
Serial.write(0);
datastreamState = datastreamIdle;
break;
}
}
void Roomba632::SendStopCommand(){
Serial.write(OCStop);
state = 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
state = roombaCleaning;
}
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
state = roombaDocking;
}
break;
}
}
void Roomba632::SendSpotCommand(){
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(OCSpot);
commandstate = startIO; //reset state
state = roombaSpotCleaning;
}
break;
}
}
void Roomba632::SendPlayMusicCommand(){
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(OCFullmode);
commandstate = startIO; //reset state
state = roombaMusic; //sending music commands is handled in the music state
}
break;
}
}
void Roomba632::_evilLights(){
}
/*
unsigned long Roomba632::_songLength(song as argument){
return songlength;
}
*/
Troubleshooting:
I added timers to the functions that send the commands. The roomba apparently needs some time to process it all (brain the size of a universe and all we ask it to do is to suck up dust) The commands for cleaning, docking, spot etc. work quite nicely like this.
The commands to start and stop the datastream are harder to diagnose. It seems that a datastream is started and stopped. I get a databurst very 15ms on the scope.

Interpreting the data is something else though. I'm not getting through the state machine so more debugging is required.
It also seems the OTA gets quite unstable. sometimes I can reach the platform from the Arduino IDE, but an upload results in an error. I will have to figure this out more thoroughly, so all that's next.
Then use the class in some neat code and expand the class with some options:
- Detect when the roomba is docked (should be simple enough by checking the serial messages);
- Add control over music and LED's;
- Maybe add full drive capabilities.
Discussions
Become a Hackaday.io Member
Create an account to leave a comment. Already have an account? Log In.