MausBot2.zip (8721Bytes)
Introduction
Well, after the couple of comments from this blog, Maus LearningBot - Family Challenge One (State Machine), I've done some reading, recoding and experimenting with some of the concepts suggested of how state machines should work. I've now dubbed this version 2 and it seems to work just as well as version 1. I'll try to explain myself as I walk through the code.
Why a State Machine?
I could have written the code for our wall challenge as straight decisions with the normal 'if..then..else' type logic. That would have been all cool and interesting in some way, but I thought that if I started writing 'states' and 'actions', then I could build up a library of things my robot could do and then link them together with some binding logic to perform various tasks. I also thought this would be better in the long run as I could write more complex decisions without having to invest time in the simple tasks.
Highlights of the Code
First off, I wrote a motor library that I hope to expand on to incorporate code for turning and automatically adjusting the motors to straighten out the robot better than my 'adjustment' constants. These constants are fair at the middle speed but rotten at the lowest speed where I get a distinct turn out of my robot. I hope to have that code in the next version of the motor library when I read up and understand the PID algorithms and how to use them on 2 motors.
typedef enum {
stateINITIALIZE = 0,
stateMOVE,
stateFINISHED
} state_t;
typedef enum {
eNONE = 0,
eOBJ_LONG,
eOBJ_SHORT,
eOBJ_BLOCK
} event_t;
typedef void (*action_t)();
Here we see the setup for our state machine. We have states that the robot is in such as INITIALIZE any starting tasks, on the MOVE, and FINISHED with the task. Next is the types of events we will generate that have an effect on the state of the robot. We have a NONE event that means nothing has happened, OBJ_LONG says we've detected an object at long distance, OBJ_SHORT for short distance and OBJ_BLOCK when we are about to be blocked from moving. Finally, I have set up a function pointer type for the action we will take at various points in our state machine.
typedef struct {
state_t nextState;
action_t actionToDo;
} stateAction_t;
stateAction_t stateMatrix[3][4] = {
{ {stateMOVE, actionInitialize}, {stateMOVE, actionInitialize}, {stateMOVE, actionInitialize}, {stateFINISHED, actionStop} },
{ {stateMOVE, actionNone}, {stateMOVE, actionMoveForward}, {stateMOVE, actionCreepForward}, {stateFINISHED, actionStop} },
{ {stateFINISHED, actionBlink}, {stateFINISHED, actionBlink}, {stateFINISHED, actionBlink}, {stateFINISHED, actionBlink} }
};
Now we have the state matrix that helps us cross reference our current state (x) with the current event (y) and tells us the next state and action to perform. I know I need to clean this matrix up a little, but for the initial coding, it works. The first line are the transitions for the INITIALIZE states, next is the transitions for the MOVE state and then the FINISHED state. For each line, there is a pair of data values for the next state to go into and the action to take based on the event that comes in.
For example: If we are in the MOVE state and the OBJ_CLOSE event occurs, then we would then transition into the {stateMOVE, actionCreepForward} element of the stateMatrix. This tells us that the next state is still the MOVE state and we should perform the actionCreepForward action to slow ourselves down.
// Set our initial state and event to run any state machine
// setup code.
currentState = stateINITIALIZE;
currentEvent = eNONE;
readSensor = 0;
// initialize Timer2 for our Event/Sensor code
noInterrupts();
TCCR2A=0;
TCCR2B=0;
TCNT2 =0;
OCR2A=31250; // compare match register 8MHz/256/1000Hz
TCCR2B |= (1 << WGM22); // CTC mode
TCCR2B |= (1 << CS22); // 256 prescaler
TIMSK2 |= (1 << OCIE2A); // enable timer compare interrupt
interrupts();
In the setup() function, we initialize our current state and event to ensure we hit the initialization action. I also decided to use Timer2 to set up a regular reading from my Ping sensor to help generate events based on the distance to our wall. Remember that our Family Challenge is to get the robot closest to a wall in front of it without knocking over the blocks of the wall.
/*
** Set our sensor indicator that it's time to read a distance
*/
ISR(TIMER2_COMPA_vect)
{
readSensor = 1;
}
void loop() {
if (readSensor)
{
getSensorReading();
readSensor = 0;
}
stateEval();
}
The timer just sets a flag indicating that it's okay to read a distance from the Ping sensor. The main loop is really very simple and probably will not change dramatically. Basically we see if we need to read the sensor and then after that, evaluate what we need to do at our current state and event. That's the real brains of the state machine and can be seen below:
/*
** Evaluate our current state and the current event to see what
** we need to do next.
*/
void stateEval()
{
stateAction_t sa = stateMatrix[currentState][currentEvent];
currentState = sa.nextState;
(*sa.actionToDo)();
}
To perform a state machine evaluation, we get our action from our matrix based on the current state and event. The next state we are going into is contained in the matrix. We set that for the future evaluations. Next we perform the action from the function pointer we set up.
One of the things I think I will change is some smarts about when to perform additional actions. I know that at this point I am probably doing repeated actions and wasting CPU cycles but for the second stab at state machines, I think I'm doing okay.
If you don't like function pointers, then you could replace the function pointer action type with another enumeration and then in this function you would use a switch statement to determine what action to perform. I like the function pointers because I wanted to remove the excess enumeration overhead and I'm tricky like that.
Actions are just very simple routines at the moment. The move forward action is just accessing the motor library and telling it to go forward as moderate speed.
void actionMoveForward()
{
DebugCode(
Serial.println("actionMoveForward");
)
motors.drive(NORMAL_SPEED, Motor::FORWARD);
displayEncoders();
}
All the code is included in the attached zip file.
Conclusion
Well, I hope this helps to understand state machines and maybe a different way of thinking about robot logic. I know that I'm going to continue down this path for a while to see if it makes things easier or more difficult with increasingly challenging problems. Maybe there are techiniques I need to learn, so as always, comments are welcome and encouraged. Let me know if this was helpful or if I'm full of beans and what I did wrong.
Maus
Per NilsB excellent request, here is the code for the ultrasonic sensor that runs periodically. I use this code to create events that drive what happens in the state machine. This is a pretty important step as it drives what state and action we will take as our robot moves along.
/*
** Perform a sensor reading based on our timer. We need an average
** of 3 sensor readings to ensure we are not getting a false zero
** in our readings. Then set our event as either a long distance,
** short distance or that we are about to crash.
*/
void getSensorReading()
{
long total = 0;
long distance = 0;
for (uint8_t x = 0; x < 3; x++) {
sensor.pulse();
total += sensor.getCentimeters();
delay(10);
}
distance = total / 3;
if (distance > SLOW_DISTANCE) {
DebugCode(
Serial.println("eOBJ_LONG event");
)
currentEvent = eOBJ_LONG;
} else if (distance > STOP_DISTANCE) {
DebugCode(
Serial.println("eOBJ_SHORT event");
)
currentEvent = eOBJ_SHORT;
} else {
DebugCode(
Serial.println("eOBJ_BLOCK event");
)
currentEvent = eOBJ_BLOCK;
}
DebugCode(
Serial.print("readSensor cm=");
Serial.println(distance);
)
}
Here we can see how the events get generated. Based on the average of 3 sensor readings, we then determine what event to create based on the distance to the wall or object. That event then helps us determine our actions and next state in the stateMatrix.