A. Brown Design

This post discusses one way to handle setting up the Zumo to perform one of three initial moves prior to entering the main body of the program.  If you go back a couple of posts, we discussed the simple state machine for a sumo competition.  In the setup, there was a section for the initial move.  The initial move is the first move you want your robot to make prior to entering the state machine and following a preset algorithm.  This could be a fixed move, in which case, you can ignore this post.  However, you may decide you want to have your robot do a different initial move based on how your competitor is set up, or in the case of BottleSumo, where the bottle is located.  In this case, you often don’t know these conditions until you are set up at the table, ready to go.

This post describes a way that you can use the three buttons to select a particular first move.  So far, we’ve only used the buttons to wait until you press a button.  In this case, we need to record the action we want to take, wait for a button, then take that action.

The basic flow is:

1. Check to see if any of the three buttons are pressed.
2. If a button is pressed, record which one.
3. Go to step 1 if no button is pressed, if not, proceed to step 4.
4. Wait until button A is pressed to signal the start of the match.
5. Execute the option selected from step 2.
6. Proceed to normal state machine.

To make it easier, I like to use the enum function to define a data type.  You cold simply use an integer to represent button A, B, and C, but why not use the actually button name?  Defining the datatype with enum is done as below:

enum ButtonChoice       // Create datatype for the button label.
{
  A,
  B,
  C
}; 

This creates a datatype  named ButtonChoice that allows values of A, B, and C.  The next step is to continuously check the buttons until one is pressed.  For this, you will need a Boolean variable to store whether or not a button has been pressed.  It gets initialized to false, as a button has not been pressed at the beginning.  As usual, I always think it is a good idea to give the user feedback by prompting on the LCD and providing beeps to know if you have pressed a button.

bool buttonPressed = false;       // Boolean whether or not a button has been pressed.
ButtonChoice buttonSelected;      // variable to store which button was pressed.

// Prompt User and Wait for Button.  Record which button is pressed.
  lcd.clear();
  lcd.gotoXY(1, 0);
  lcd.print("Press:");
  lcd.gotoXY(0, 1);
  lcd.print("A  B  C");
  do {
    if (buttonA.isPressed()) {
      buttonSelected = A;
      buttonPressed = true;
      buzzer.playFrequency(300, 200, 15);
    }
    if (buttonB.isPressed()) {
      buttonSelected = B;
      buttonPressed = true;
      buzzer.playFrequency(440, 200, 15);
    }
    if (buttonC.isPressed()) {
      buttonSelected = C;
      buttonPressed = true;
      buzzer.playFrequency(600, 200, 15);
    }
  } while (!buttonPressed); 

At this point, you’ve selected with variant you want to run, you just need to wait till you press the button to start, and run the first move.  for this example, I made the first move options very simple.  If A is pressed, the robot moves forward, if B is pressed, the robot moves backwards, and if C is pressed, the robot turns.  In practice, you probably want something more specific, like targeting a region of the table, moving out of the way, etc.

// Wait for button and execute the selection.
lcd.clear();
lcd.print("Press A");
lcd.gotoXY(0, 1);
lcd.print("to Go!");
buttonA.waitForButton();
buzzer.playFrequency(440, 50, 15);
lcd.clear();

switch (buttonSelected) {
  case A:
    lcd.print("Case A");
    motors.setSpeeds(200, 200);
    delay(1000);
    motors.setSpeeds(0, 0);
    break;
  case B:
    lcd.print("Case B");
    motors.setSpeeds(-200, -200);
    delay(1000);
    motors.setSpeeds(0, 0);
    break;
  case C:
    lcd.print("Case C");
    motors.setSpeeds(-200,200);
    delay(1000);
    motors.setSpeeds(0,0);
    break;
}  

When putting this into your sumo state machine, you will clearly have to add other items such as the initial state to start in, light sensor calibrations, initializations, etc.  Putting the whole example program together:

#include <Zumo32U4.h>
Zumo32U4LCD lcd;
Zumo32U4Motors motors;
Zumo32U4ButtonA buttonA;
Zumo32U4ButtonB buttonB;
Zumo32U4ButtonC buttonC;
Zumo32U4Buzzer buzzer;

enum ButtonChoice       // Create datatype for the button label.
{
  A,
  B,
  C
};

void setup() {
  bool buttonPressed = false;       // Boolean whether or not a button has been pressed.
  ButtonChoice buttonSelected;      // variable to store which button was pressed.

  // Prompt User and Wait for Button.  Record which button is pressed. 
  lcd.clear(); 
  lcd.gotoXY(1, 0); 
  lcd.print("Press:"); 
  lcd.gotoXY(0, 1); 
  lcd.print("A  B  C"); 
  do { 
    if (buttonA.isPressed()) { 
      buttonSelected = A; 
      buttonPressed = true; 
      buzzer.playFrequency(300, 200, 15); 
    } 
    if (buttonB.isPressed()) { 
      buttonSelected = B; 
      buttonPressed = true; 
      buzzer.playFrequency(440, 200, 15); 
    } 
    if (buttonC.isPressed()) { 
      buttonSelected = C; 
      buttonPressed = true; 
      buzzer.playFrequency(600, 200, 15); 
    } 
  } while (!buttonPressed); 
  delay(500);   
  
  // Wait for button and execute the selection. 
  lcd.clear(); 
  lcd.print("Press A"); 
  lcd.gotoXY(0, 1); 
  lcd.print("to Go!"); 
  buttonA.waitForButton(); 
  buzzer.playFrequency(440, 50, 15); 
  lcd.clear(); 
  
  switch (buttonSelected) { 
    case A: 
      lcd.print("Case A"); 
      motors.setSpeeds(200, 200); 
      delay(1000); 
      motors.setSpeeds(0, 0); 
      break; 
    case B: 
      lcd.print("Case B"); 
      motors.setSpeeds(-200, -200); 
      delay(1000); 
      motors.setSpeeds(0, 0); 
      break;     
    case C: 
      lcd.print("Case C"); 
      motors.setSpeeds(-200,200); 
      delay(1000); 
      motors.setSpeeds(0,0); 
      break; 
  } 
} 

void loop() { 
  // Put the rest of the state machine here. 
} 

This post will talk about a simple state machine that could be used for robot sumo.  The target here is the Robofest Senior BottleSumo rules, so the state machine is adapted for long searching on large tables.  If you want to do international sumo rules, the state diagram would look a bit different.

A state machine is a device that can be in a specific condition based on previous conditions and the current value of its inputs.  One of the easiest ways to describe the behavior of a state machine is through a state diagram.  A state diagram depicts each state with a circle, then connects the states with arrows to show how the device can transfer between different states, and the conditions required for the device to change states.  You can think of it a bit like a Furby where each behavior the Furby has is a particular state.  There are things you can do to the Furby that will cause it to go from one state to another.

A very basic BottleSumo state diagram is shown below.  Note that this is just the beginning.  Personally, I’d add many other states and transitions between states to optimize performance.

A description of each state found below:

• Start – Initialize global variables, initialize sensors, perform a light sensor calibration, etc.  This would be the stuff done in the setup().
• Initial Move – The BottleSumo rules have a specific start condition.  This state would perform the unknown start condition, and maneuver the robot to a point on the table where it can really begin the state machine sequence.  Since this is performed only once, it also would be in the setup().
• Scan – Pretty much what it says.  Look around for targets.  If the robot sees a target, transition to Rotate to Target.  If the robot sees nothing, the robot should go for a walk.
• Rotate to Target – This assumes that a target is in site, this state rotates to face the target.  After this is complete, the robot should charge the target.
• Charge the Target – Ramming Speed!  You are currently facing the target, now is the time to charge it.  This state drives the robot hard until it sees the edge of the table.
• Recover from Edge – The only way that the robot enters this state is having driven to an edge of the table.  At this point, you could square to the edge, backup, rotate, then go into Scan again.
• Wander – Basically just going for a walk around the table.  This diagram assumes you are wandering at a slower speed, and monitoring the downward facing light sensors and proximity sensors.  If you wander to an edge of the table, you Recover from Edge.  If you see a target while wandering, you Rotate to Target.

As I said above, this is really just the beginning.  You can add a lot more complexity as you see fit.  To create a state machine, it is very useful to create a datatype that defines the states.  You could simply assign each state a number and refer to the states as integers.  However, I’m old…. It is much easier to create a datatype that uses the state name.  To do this, you need to use the enum command.

enum State
{
  stateScanning,            // Robot scans from static position.
  stateWander,              // Robot wanders around.  
  stateRotate2Target,       // Assumes target is found, will rotate to largest prox value.
  stateChargeTarget,        // Charges a target.  
  stateEdgeRecovery         // Recovers from finding an edge.
}; 

This creates a new datatype called “State” that can consist of only the values listed.  This allows us to declare a variable of this datatype later that will store the current value of the state.  You can also create variables that can store previous values of the state as well.   Note that I did not include the states for Start and Initial Movement.  These are simple enough and can be handled in the setup() individually.

The next thing we need to do is to create the variable of datatype State, and store the first state of the robot that we want to execute once the loop() begins.  We can do this with the following statement:

State currentState = stateScanning;        // Declare and initialize the state variable of type State 

This command creates a variable named “currentState” that is type State (uppercase s), and we are assigning that variable the value of stateScanning, which is the state we want to be in when we enter loop() for the first time.

You would then write the setup() function, which does all the normal stuff we would do such as initialize sensors, light sensor calibrate, etc, but then we would add the start condition and the first move.  After the setup is complete, we begin the loop().  Each time through the loop, the robot needs to look at the value of the variable state, execute the commands associated with the value, change states, then begin the loop again (which should be under a new state).  This can be done easily with the switch case command.  Details for this particular command can be found at:

https://www.arduino.cc/en/Reference/SwitchCase

Below is an example, with a lot of the functional code for each state removed, for the basic state machine described above.

#include <Zumo32U4.h>
Zumo32U4LCD lcd;
Zumo32U4ProximitySensors proxSensors;
Zumo32U4ButtonA buttonA;
Zumo32U4Motors motors;
Zumo32U4Encoders encoders;
Zumo32U4Buzzer buzzer;
Zumo32U4LineSensors lineSensors;

// setup possible values of the robot states
enum State
{
  stateScanning,            // Robot scans from static position.  
  stateWander,              // Robot wanders around.  
  stateRotate2Target,       // Assumes target is found, will rotate to largest prox value
  stateChargeTarget,        // Charges a target. 
  stateEdgeRecovery         // Recovers from finding an edge.
};

// Global Variables
unsigned int lineSensorValues[3];   // line sensor values.  Array of [left, middle, right]
State currentState = stateScanning;        // Declare and initialize the state variable of type State

void setup() {
  proxSensors.initThreeSensors();
  lineSensors.initThreeSensors();
  calLightSensors(5);
  
  // Wait for Start
  lcd.clear();
  lcd.print("Press A");
  buttonA.waitForButton();
  lcd.clear();
  // Add Code for Start Condition Here
  // Add Code for the First Move Here
}

void loop() {

  switch (currentState) {
    case stateScanning:
      {
        // Add code for the scanning step
        if(ADD CONDITIONAL FOR FINDING A TARGET) {
          currentState=stateRotate2Target;
        } else {
          currentState=stateWander;
        }
      }
      break;

   case stateRotate2Target:
      {
        //Add code for rotating to face a target  
        currentState=stateChargeTarget;
      }
      break;
    case stateChargeTarget:
      {
        // Add code for charging a target
        currentState=stateEdgeRecovery;
      }
      break;

    case stateEdgeRecovery:
      {
        // Add code for recovering from hitting the edge of the table.
        currentState=stateScanning;
      }
      break;

    case stateWander:
      {
        // Add code for wander routine
        if(ADD CONDITION FOR IF THE ROBOT IS AT THE EDGE OF TABLE) {
          currentState=stateEdgeRecovery;
        } else if(ADD CONDITION FOR FINDING A TARGET) {
          currentState=stateRotate2Target;
        } 
      }
      break;
  } // This ends the switch case block
} 

There are a lot of commented out sections that should be filled in for the behavior or each state.  Also, the conditions for some of the state changes are clearly not detailed.  However, this should give you the starting point for putting stuff together.  I would encourage you to make separate test programs for the various states prior to putting them in the full state machine.  This allows you to create and test each state by themselves prior to dealing with transitioning between many states.