Mindstorms State Machines

1. A structured method for handling tasks and events using RoboLab and NQC Brian Smith bbsmith@twmi.rr.com Mindstorms State Machines

2. A state machine is a model of the behavior of a system – in this case an autonomous robot – consisting of states, transitions, and actions What is a State Machine?

3. What the robot is doing Following a line, avoiding an obstacle, turning, or operating an arm are states Doing nothing is also a state States

4. A change in condition that causes the robot to change states A bumper touch, an increase or decrease in a light sensor value, or receiving a message from another robot are transitions Transitions

5. How the robot changes states, or what the robot does in a state Stopping all motors to change the robot's state from moving to stopped or following a line in the line following state are actions Actions

6. Step 1 - Define the states State Machine ExampleA Light Seeking Robot Seek Locate bright light

7. Step 1 - Define the states State Machine ExampleA Light Seeking Robot Seek Avoid Avoid an obstacle

8. Step 1 - Define the states State Machine ExampleA Light Seeking Robot Seek Avoid Follow Follow bright light

9. Step 2 - Define the transition events State Machine ExampleA Light Seeking Robot Seek Avoid Light High Follow Sensor detect increase in light

10. Step 2 - Define the transition events State Machine ExampleA Light Seeking Robot Sensor detects decrease in light Seek Avoid Light Low Light High Follow

11. Step 2 - Define the transition events State Machine ExampleA Light Seeking Robot Obstacle Seek Avoid Light Low Sensor detects an obstacle in the robot's path Light High Follow

12. Step 3 - Define the transitions State Machine ExampleA Light Seeking Robot Obstacle Seek Avoid Light Low Light High Follow Light High from Seek – go to Follow

13. Step 3 - Define the transitions State Machine ExampleA Light Seeking Robot Obstacle Seek Avoid Light Low Light High Follow Light Low from Follow – go to Seek

14. Step 3 - Define the transitions State Machine ExampleA Light Seeking Robot Obstacle Seek Avoid Light Low Light High Follow Obstacle from Follow or Seek – go to Avoid

15. Step 3 - Define the transitions State Machine ExampleA Light Seeking Robot Obstacle Seek Avoid Light Low Light High Follow Avoid Complete – go to Seek

16. We now have a model of the behavior of a light seeking robot This model doesn't require any particular robot design, sensors, or programming language All the robot needs is a way to move, a way to detect a change in light, and a way to detect an obstacle in it's path State Machine ExampleA Light Seeking Robot

17. We'll use an RCX differential drive robot with a touch bumper and a light sensor. State Machine ExampleA Light Seeking Robot

18. To write the program in RoboLab we need: State Machine ExampleA Light Seeking Robot

19. To write the program in RoboLab we need: A task to watch the touch sensor State Machine ExampleA Light Seeking Robot

20. To write the program in RoboLab we need: A task to watch the touch sensor A task to watch the light sensor State Machine ExampleA Light Seeking Robot

21. To write the program in RoboLab we need: A task to watch the touch sensor A task to watch the light sensor A subroutine to do the Seek actions State Machine ExampleA Light Seeking Robot

22. To write the program in RoboLab we need: A task to watch the touch sensor A task to watch the light sensor A subroutine to do the Seek actions A subroutine to do the Follow actions State Machine ExampleA Light Seeking Robot

23. To write the program in RoboLab we need: A task to watch the touch sensor A task to watch the light sensor A subroutine to do the Seek actions A subroutine to do the Follow actions A subroutine to do the Avoid actions State Machine ExampleA Light Seeking Robot

24. To write the program in RoboLab we need: A task to watch the touch sensor A task to watch the light sensor A subroutine to do the Seek actions A subroutine to do the Follow actions A subroutine to do the Avoid actions A task to handle switching between states State Machine ExampleA Light Seeking Robot

25. To write the program in RoboLab we need: A task to watch the touch sensor A task to watch the light sensor A subroutine to do the Seek actions A subroutine to do the Follow actions A subroutine to do the Avoid actions A task to handle switching between states Containers for current and next states State Machine ExampleA Light Seeking Robot

26. RoboLab provides 23 containers for programmers to use – red (0), blue(1), yellow(2) and user containers 3 to 22. User container 21 will hold a value representing the current state. Container 22 will hold the value representing the next state. The values assigned will be 1 for Seek, 2 for Avoid, and 3 for Follow. State Machine ExampleA Light Seeking RobotRoboLab Containers

27. State Machine ExampleA Light Seeking RobotLight Task - RoboLab

28. State Machine ExampleA Light Seeking RobotObstacle Task - RoboLab

29. State Machine ExampleA Light Seeking RobotFollow Subroutine - RoboLab

30. State Machine ExampleA Light Seeking RobotAvoid Subroutine - RoboLab

31. State Machine ExampleA Light Seeking RobotSeek Subroutine - RoboLab

32. State Machine ExampleA Light Seeking RobotState Change Task - RoboLab

33. State Machine ExampleA Light Seeking RobotComplete RoboLab program

34. The robot behavior described by our state machine model doesn't require a specific language. We could write the program in NQC, Java, or any other language that has support for our controller The following slides show how it could be implemented in NQC State Machine ExampleNQC Implementation

35. Instead of containers, NQC uses integer (int) variables to hold values. '#define' allows you to substitute words for values to make your program easier to read – the value ‘1’ can be replaced by the word ‘stateSeek’ in your program #define stateUndefined -1 #define stateInitialization 0 #define stateSeek 1 #define stateAvoid 2 #define stateFollow 3 int nCurrState; int nNextState; State Machine ExampleNQC Implementation

36. // // task to watch the light sensor // wait for high light, set state to 'stateFollow' // --- then --- // wait for low light, set state to 'stateSeek' // NOTE: // this assumes that robot is not facing the bright light when this task starts // task taskLight() { // loop forever while(true) { // loop until a bright light is found while(lightSensor < 45) { } // stop motors SetOutput(motorLeft + motorRight, OUT_OFF); // turn A and C off // set state to follow nNextState = stateFollow; // wait 1 second Wait(100); // loop until bright light is lost while(lightSensor >= 45) { } nNextState = stateSeek; } } State Machine ExampleA Light Seeking RobotLight Task - NQC

37. // // task to watch the bumper sensor // on bump, change next state to 'stateAvoid' // task taskObstacle() { // monitor forever while(true) { // loop until an obstacle is found while(obstacleSensor == 0) { } // set state to avoid nNextState = stateAvoid; } } State Machine ExampleA Light Seeking RobotObstacle Task - NQC

38. // // subroutine for follow state action // no need to monitor for state changes - this just sets the // motors forward at full power and exits // sub subFollow() { // set and display current state nCurrState = stateFollow; SetUserDisplay(nCurrState, 0); PlaySound(SOUND_DOUBLE_BEEP); // set both motors forward and exit // they will stay forward until something else changes their direction SetDirection(motorLeft + motorRight, OUT_FWD); SetOutput(motorLeft + motorRight, OUT_FULL); On(motorLeft + motorRight); } State Machine ExampleA Light Seeking RobotFollow Subroutine - NQC

39. // // subroutine for avoid state action // back up, turn right or left, then set next state to seek // this subroutine doesn't look for state changes - it runs to completion // sub subAvoid() { // set and display current state nCurrState = stateAvoid; SetUserDisplay(nCurrState, 0); PlaySound(SOUND_DOUBLE_BEEP); // reverse for 1 second SetDirection(motorLeft + motorRight, OUT_REV); SetOutput(motorLeft + motorRight, OUT_FULL); On(motorLeft + motorRight); Wait(100); // turn right or left for 1 second if(Random(1) == 0) { SetDirection(motorLeft, OUT_FWD); } else { SetDirection(motorRight, OUT_FWD); } Wait(100); // stop both motors Off(motorLeft + motorRight); nNextState = stateSeek; } State Machine ExampleA Light Seeking RobotAvoid Subroutine - NQC

40. // subroutine for seek state action // drive forward for a random time <= 5 seconds, then spin for a random time <= 2 seconds // this subroutine needs to allow itself to be interrupted by state changes // sub subSeek() { // set and display current state nCurrState = stateSeek; SetUserDisplay(nCurrState, 0); PlaySound(SOUND_DOUBLE_BEEP); int mSecs = 0; // loop until next state changes while(true) { // forward <= 5 seconds mSecs = Random(500); ClearTimer(T1); // forward <= 5 seconds SetDirection(motorLeft + motorRight, OUT_FWD); SetOutput(motorLeft + motorRight, OUT_FULL); On(motorLeft + motorRight); // this loops until the timer reached the desired value or the state changes while((Timer(T1) <= mSecs) && (nNextState == nCurrState)) { } // break while(true) loop if state has changed if(nNextState != nCurrState) { break; } State Machine ExampleA Light Seeking RobotSeek Subroutine - NQC

41. // turn <= 2 seconds mSecs = Random(200); ClearTimer(T1); // turn right or left <= 2 seconds if(Random(1) == 0) { SetDirection(motorLeft, OUT_REV); // reverse left motor } else { SetDirection(motorRight, OUT_REV); // reverse right } On(motorLeft + motorRight); // this loops until the timer reached the desired value or the state changes while((Timer(T1) <= mSecs) && (nNextState == nCurrState)) { } // break while(true) loop if state has changed if(nNextState != nCurrState) { break; } } // gets here if state has changed (break statements) // turn right and left motors off Off(motorLeft + motorRight); return; } State Machine ExampleA Light Seeking RobotSeek Subroutine – NQC (cont)

42. // initial setup, handle state changes task main() { // One-time startup code SetSensor(obstacleSensor, SENSOR_TOUCH); SetSensor(lightSensor, SENSOR_LIGHT); nCurrState = stateUndefined; nNextState = stateInitialization; // Continuous loop for state machine while (true) { // don't change state unless needed if(nCurrState == nNextState) { continue; } // change to next state switch (nNextState) { // initialization to start tasks and set first running state case stateInitialization: start taskLight; start taskObstacle; nNextState = stateSeek; break; State Machine ExampleA Light Seeking RobotState Change Task - NQC

43. // go into seek state (from avoid or follow) case stateSeek: subSeek(); break; // go into avoid state (from seek or follow) case stateAvoid: subAvoid(); break; // go into follow state (from seek only) case stateFollow: if(nCurrState == stateSeek) { subFollow(); } else { nNextState = nCurrState; } break; // Should never get here. Play warning message and restart default: PlaySound(SOUND_DOUBLE_BEEP); SetUserDisplay(9999, 0); Wait(100); stop taskLight; stop taskObstacle; nCurrState = stateUndefined; nNextState = stateInitialization; break; } } } State Machine ExampleA Light Seeking RobotState Change Task – NQC (cont)