State Machine example code

// State Machine example for RoboSumo
 
// function prototypes
void forward();
void reverse();
void motors(int, int, int, int);
float distance();

int state = 1;
 
void setup()
{
  // Motor pins
  pinMode(2, OUTPUT); // Left motor foward
  pinMode(3, OUTPUT); // Left motor reverse
  pinMode(4, OUTPUT); // Right motor foward
  pinMode(5, OUTPUT); // Right motor reverse
   
  pinMode(6, OUTPUT); // trigger pin for rangefinder
  
  // Assume colour sensors are
  // FLS: front left sensor on A0
  // FRS: front right sensor on A1
}

void loop()
{
  unsigned long t;
  int fls, frs;
  float rf;
  
  while (state == 1)
  {
      // read sensors
      fls = analogRead(A0);
      frs = analogRead(A1);
      rf = distance();
      
      // set motors
      forward();
      
      // Change state?
      if (fls > 512) state = 2;
      if (frs > 512) state = 3;
      if (rf < 60) state = 4;
  }
  
  while (state == 2)
  {
      // read sensors
      frs = analogRead(A1);
      
      // set motors
      motors(0, 0, 1, 0); // LM stop, RM forward
      
      // Change state?
      if (frs > 512) state = 5;
  }
  
  while (state == 3)
  {
      // read sensors
      fls = analogRead(A0);
      
      // set motors
      motors(1, 0, 0, 0); // LM forward, RM stop
      
      // Change state?
      if (fls > 512) state = 5;
  }
  
  while (state == 4)
  {
      // read sensors
      rf = distance();
      
      // set motors
      forward();
      
      // Change state?
      if (rf > 60) state = 6;
  }
  
  t = millis();
  while (state == 5)
  {
      // read sensors
      // n/a
      
      // set motors
      reverse();
      
      // Change state?
      if (millis() - t > 2300) state = 6;
  }
  
  t = millis();
  while (state == 6)
  {
      // read sensors
      rf = distance();
      
      // set motors
      motors(1, 0, 0, 1); // LM forward, RM reverse
      
      // Change state?
      if (rf < 60) state = 4;
      if (millis() - t > 5000) state = 1;     
  }  
}
 
float distance()
{
  int duration;
  float dist;
   
  // Send trigger pulse
  digitalWrite(6, HIGH);
  delayMicroseconds(20);
  digitalWrite(6, LOW);
  
  // Wait for start of echo pulse
  while (digitalRead(7) == 0);
  
  duration = 0;
  while (digitalRead(7) == 1)
  {
    duration = duration + 10;
    delayMicroseconds(10);
  }
  
  dist = 0.5 * 1e-6 * duration * 340.0;
   
  return dist;
}
 
void motors(int lmf, int lmr, int rmf, int rmr)
{
  digitalWrite(2, lmf);
  digitalWrite(3, lmr);
  digitalWrite(4, rmf);
  digitalWrite(5, rmr);
}
 
void forward()
{
  motors(1, 0, 1, 0);
}
 
void reverse()
{
  motors(0, 1, 0, 1);
}
This entry was posted in Uncategorized. Bookmark the permalink.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s