State machine example from class – “Shoo Robot”

//
// State Machine Example for Arduino Nano
// Written by Ted Burke - 14-11-2019
//

// Variables for sensor readings
int fls, frs, bls, brs; // colour sensors
float dl, dr, dc;       // left , right and centre distance

int state = 1;

void setup()
{
  pinMode(2, OUTPUT); // LM forward
  pinMode(3, OUTPUT); // LM reverse
  pinMode(4, OUTPUT); // RM forward
  pinMode(5, OUTPUT); // RM reverse
}

void loop()
{
  // Read all colour sensors
  fls = analogRead(0);
  frs = analogRead(1);
  bls = analogRead(2);
  brs = analogRead(3);

  // Read distance sensors
  dl = readLeftDistance();
  dr = readRightDistance();
  dc = readCentreDistance();

  // Implement the current state
  if (state == 1) // forward to line
  {
    // actuators
    motors(1, 1); // both motors forward

    // state transitions
    if (fls > 800) state = 2;
    if (frs > 800) state = 3;
    if (dl < 6.0) state = 11;
    if (dr < 6.0) state = 12;
    if (dc < 6.0) state = 9;
  }
  else if (state == 2) // straighten up right side
  {
    // actuators
    motors(0, 1); // LM stop, RM forward

    // state transitions
    if (frs > 800) state = 4;
  }
  else if (state == 3) // straighten up left side
  {
    // actuators
    motors(1, 0); // LM forward, RM stop

    // state transitions
    if (fls > 800) state = 4;
  }
  else if (state == 4) // turn 90 degrees anti-clockwise
  {
    // actuators
    motors(-1, 1); // LM reverse, RM forward

    // state transitions
    delay(1200);
    state = 5;
  }
  else if (state == 5) // forward to corner
  {
    // actuators
    motors(1, 1); // LM forward, RM forward

    // state transitions
    if (fls > 800) state = 6;
  }
  // etc etc
}

void motors(int left, int right)
{
  // Set left motor direction
  if (left < 0)
  {
    digitalWrite(2, LOW);
    digitalWrite(3, HIGH); // LM reverse
  }
  else if (left > 0)
  {
    digitalWrite(2, HIGH); // LM forward
    digitalWrite(3, LOW);
  }
  else
  {
    digitalWrite(2, LOW); // LM stop
    digitalWrite(3, LOW);
  }

  // Set right motor direction
  if (right < 0)
  {
    digitalWrite(4, LOW);
    digitalWrite(5, HIGH); // RM reverse
  }
  else if (right > 0)
  {
    digitalWrite(4, HIGH); // RM forward
    digitalWrite(5, LOW);
  }
  else
  {
    digitalWrite(4, LOW); // RM stop
    digitalWrite(5, LOW);
  }
}
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