Close

Spring Cleaning

A project log for 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.

Simon JansenSimon Jansen 03/30/2022 at 13:540 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 if(p_spotFlag){
                SendSpotCommand();
            }
            else if(p_playMusicFlag){
                SendPlayMusicCommand();
            }    
            if(millis() > p_undockedTimer){
                if(docked){
                    roombaState = roombaHome;
                }
            }
            break;
        case roombaDriving:
            p_driveFlag = false;
            roombaState = roombaIdle;
            break;
        case roombaMusic:
            p_playMusicFlag = false;
            //time music or poll state
            roombaState = roombaIdle;
            break;
    }
    //State machine for getting data from Roomba in docked and undocked state.
    //
    static unsigned long messagetimeouttimer, commandTimer;
    static char* pos;
    static char buffer[7];
    static char i, j, errorCounter;
    static char chargingHeaderBuffer[10], chargingMessageBuffer[80];
    static byte databuffer[30]; // 12 packetID's + 18 databytes
    static byte checksum;
    
    switch (dataState){
        //Idle: try to start datastream
        case dataIdle:
            //try to start datastream
            dataState = datastreamStart;
        break;
        //Handle serial charging message
        //Example:
        //"bat:   min 16  sec 35  mV 16198  mA 1102  tenths-deg-C 223  mAH 2496  state 5"
        case chargingMessageWaitingForHeader:
            if(Serial.available()){
                if(Serial.read() =='b'){
                    dataState = chargingMessageReadingHeader;
                    //p_requestStopDatastreamFlag = true;
                    j=0;
                }
            }
            //timeout?
            else if (millis() > messagetimeouttimer){
                   //start datastream to figure out what it's up to
                dataState = datastreamStart;
            }
        break;
        case chargingMessageReadingHeader:
            if (Serial.available()){
                chargingHeaderBuffer[j] = Serial.read();
                j++;
                if (j==10){
                    if (strncmp(chargingHeaderBuffer, "at:   min ",10)==0){ 
                        dataState = chargingMessageReadingMessage;
                        messagetimeouttimer = millis() + CHARGINGMESSAGETIMEOUT; //reset timer
                    }
                    else{
                        dataState = chargingMessageWaitingForHeader;
                    }
                    j = 0;
                }
            }
            //timeout?
            else if (millis() > messagetimeouttimer){
                   //start datastream to figure out what it's up to
                dataState = datastreamStart;
            }
            break;
        case chargingMessageReadingMessage:
            if(Serial.available()){
                chargingMessageBuffer[j] = Serial.read();
                if (j==79){
                    j=0;
                    dataState = chargingMessageReadingHeader;
                }
                if (chargingMessageBuffer[j] == '\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; //Gaat nog niet goed. Is in tienden! Dus delen door 10
                        }
                        if (j== 11){
                            strcpy(buffer, pos);
                            batteryCapacity = atol(buffer);
                        }
                        if (j== 13){
                            strcpy(buffer, pos);
                            chargingState = atoi(buffer);
                        }
                        j++;
                        pos = strtok(NULL, " ");
                    }
                    //state = roombaHome;
                    //charging = true;
                    //docked = true;
                    //p_requestStopDatastreamFlag = true;                
                    dataState = chargingMessageReadingHeader;
                    j = 0;
                }
                j++;
            }
            //timeout?
            else if (millis() > messagetimeouttimer){
                   //start datastream to figure out what it's up to
                dataState = datastreamStart;
            }
        break;
        //Handle datastream
        case datastreamStart:
            Serial.write(OCstart); //Roomba will change to passive mode
            //maybe clear data?
            commandTimer = millis() + COMMANDSTARTTIMEOUT;        
            dataState = datastreamStarting;
        break;
         case datastreamStarting:
            if (millis() > commandTimer){
                //setup datastream
                Serial.write(OCDatastream); //start stream
                Serial.write(12); //12 packets
                Serial.write(15); //1- dirt detect U 1
                Serial.write(21); //2- Charging state U 1
                Serial.write(22); //3- voltage U 2
                Serial.write(23); //4- current S 2
                Serial.write(24); //5- Battery temp S 1
                Serial.write(25); //6- Battery charge U 2
                Serial.write(26); //7- Battery capacity U 2
                Serial.write(34); //8- Charging sources available U 1
                Serial.write(35); //9- OI mode U 1
                Serial.write(37); //10- Song playing? U 1
                Serial.write(43); //11- Wheel encoder counts left S 2
                Serial.write(44); //12-Wheel encoder counts right S 2
                //serial write startDatastream
                dataState = datastreamWaitingForHeader;
                //Set timer
                messagetimeouttimer = millis() + DATASTREAMTIMEOUT;
            }
            break;
        case datastreamWaitingForHeader:
            //disentangle datastream and do checksum for valid data. 
            if (Serial.available()){
                byte serialByte= Serial.read();
                if(serialByte == 19){ //Header for datastream (save to variable to do multiple checks for charging message etc)
                    dataState = datastreamWaitingForNbytes;
                    messagetimeouttimer = millis() + DATASTREAMTIMEOUT; //reset timer
                }            
                //timeout
                else if (millis() > messagetimeouttimer){
                    messagetimeouttimer = millis() + DATASTREAMTIMEOUT; //reset timer
                    errorCounter++;
                    if (errorCounter == DATASTREAMMAXERROR){
                        dataState = datastreamStopping;
                    }
                }
            }
            break;    
        case datastreamWaitingForNbytes:
            if (Serial.available()){
                if(Serial.read() == 30){ //number of bytes
                    dataState = datastreamReadingData;
                    i = 0;
                    checksum = 49; //19 + 30 starting value of checksum
                    messagetimeouttimer = millis() + DATASTREAMTIMEOUT; //reset timer
                }    
                else { //number of bytes should imediately follow header. Else, header was just a rando 19 value
                    //Go back to finding the next header
                    dataState = datastreamWaitingForHeader;
                }
            }    
            //timeout
            else if (millis() > messagetimeouttimer){
                messagetimeouttimer = millis() + DATASTREAMTIMEOUT; //reset timer
                errorCounter++;
                if (errorCounter == DATASTREAMMAXERROR){
                    dataState = datastreamStopping;
                }
            }
            break;
        case datastreamReadingData:
            if(Serial.available()){
                databuffer[i] = Serial.read();
                checksum += databuffer[i];
                if(i==29){
                    dataState = datastreamCheckingData;
                    messagetimeouttimer = millis() + DATASTREAMTIMEOUT; //Reset timer
                }
                i++;
            }
            //timeout
            else if (millis() > messagetimeouttimer){
                messagetimeouttimer = millis() + DATASTREAMTIMEOUT; //reset timer
                errorCounter++;
                if (errorCounter == DATASTREAMMAXERROR){
                     dataState = datastreamStopping;
               }
            }
            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];
                    chargingState = databuffer[3];
                    batteryVoltage = (databuffer[5] << 8) | databuffer[6];
                    current = (databuffer[8] << 8) | databuffer[9];
                    batteryTemp = databuffer[11];
                    batteryCharge = (databuffer[13] << 8) | databuffer[14];
                    batteryCapacity = (databuffer[16] << 8) | databuffer[17];
                    docked = bitRead(databuffer[19],1);
                    OImode = databuffer[21];
                    songPlaying = databuffer[23];
                    encoderLeft = (databuffer[25] << 8) | databuffer[26];
                    encoderRight = (databuffer[28] << 8) | databuffer[29];
                    //dataReady = true;
                    errorCounter = 0;
                    if(docked){

                        dataState = datastreamStopping;
                    }
                    else{
                        dataState = datastreamWaitingForHeader;
                    }
                }
                else{
                    //checksum failed
                    errorCounter++;
                    if (errorCounter == DATASTREAMMAXERROR){
                         dataState = datastreamStopping;
                   }
                }
            }
            //timeout
            else if (millis() > messagetimeouttimer){
                dataState = datastreamWaitingForHeader;
                errorCounter++;
                if (errorCounter == DATASTREAMMAXERROR){
                     dataState = datastreamStopping;
                }
            }
            break;
        case datastreamStopping:
            //handle last message
            //serial write stop datastream
            Serial.write(OCPauseDatastream);
            Serial.write(0);
            Serial.write(OCStop);
             dataState = chargingMessageWaitingForHeader;
            messagetimeouttimer = millis() + CHARGINGMESSAGETIMEOUT; //reset timer
            errorCounter = 0; 
            break;
     }
}

void Roomba632::SendStopCommand(){
    static bool commandstate;
    static unsigned long commandTimer;
    if (!commandstate){
        Serial.write(OCStop);
        commandstate = true;
        commandTimer = millis() + COMMANDSTARTTIMEOUT;
    }
    else{        
        if (millis() > commandTimer){
            commandstate = false;
            roombaState = 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
                roombaState = roombaCleaning;
                p_undockedTimer = millis()+DOCKEDDEBOUNCETIME;
            }
            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
                roombaState = roombaDocking;
                p_undockedTimer = millis()+DOCKEDDEBOUNCETIME;
            }
            break;                    
    }
}

Previously, I had trouble keeping the (MQTT-) connection stable. This would only happen when the roomba was docked. And when the connection returned it would always have a full Serial.available() buffer. I suspected this had something to do with interrupts and the incoming serial buffer. I tried to use a bigger Serial Rx buffer, but this didn't help. It looks to be stable now, so I'm hoping it's sorted.

Neat! Now I can finally begin with positioning data :)


-2022-04-02; Small update: Uncomment the line

//Serial.write(OCStop);

in the datastreamStopping case. Otherwise there will be no charging message, so it will timeout - start datastream - docked==true - stop datastream - repeat. 

Updated header and source files are uploaded.

I'm still having stability / connection issues though and I don't think its directly related to the interpretation of the incoming message. I disabled all interpretation and just clear the incoming buffer with a Serial.read(); statement. This didn't help. 

In the main sketch I'm using https://arduinojson.org/ to construct my MQTT-message. This is quite strict on memory size declarations and I hadn't updated this with some changes I made. When I fixed this it seemed a permanent fix until it didn't.

It does have something to do with being docked. Whenever it's "stuck", I can just remove it from its base and it will reconnect immediately. Could it be a hardware issue? Or something obscure in the way the ESP8266 handles incoming Serial comms?

Discussions