Close

Classy Roomba632

A project log for Old Roomba, new tricks

Add mapping & MQTT-interfacing with ESP8266 and an IMU

simon-jansenSimon Jansen 01/30/2022 at 19:490 Comments

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