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