State machine example from today’s class

Posted on November 23, 2023 by batchloaf 
//
// Description: State machine example for Arduino Nano
// Author: Ted Burke
// Date: 23-Nov-2023
//
//  LM: left motor (pins D2, D3)
//  RM: right motor (pins D4, D5)
//  fls: front left sensor (pin A0)
//  frs: front right sensor (pin A1)
//  

int state = 1; // global state variable

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

void loop()
{
  int fls, frs; // sensor variables

  // Read all sensors and store in variable
  fls = analogRead(0);
  frs = analogRead(1);

  // State machine
  if (state == 1) // Forward on the road
  {
    // actuators
    motors(1, 1); // both motors forward

    // state transitions
    if (fls > 512) state = 2;
    if (frs > 512) state = 3;
  }
  else if (state == 2) // Veer right
  {
    // actuators
    motors(1, 0); // left motor forward, right motor stop

    // state transitions
    if (fls <= 512) state = 1;
  }
  else if (state == 3) // Veer left
  {
    // actuators
    motors(0, 1); // left motor stop, right motor forward

    // state transitions
    if (frs <= 512) state = 1;
  }
}

void motors(int left, int right)
{
  // Set the motor control pins for the specified motor directions
  digitalWrite(2, left > 0); // LM forward
  digitalWrite(3, left < 0); // LM reverse
  digitalWrite(4, right > 0); // RM forward
  digitalWrite(5, right < 0); // RM reverse
}
Posted in Uncategorized | Leave a comment

State Machine code examples

Posted on November 24, 2022 by batchloaf 
//
// State machine example
// Most basic version
// Written by Ted Burke
// Last updated 24-Nov-2022
//

#define FAN_MOTOR_PIN 2
#define FAN_LED_PIN 5
#define HAND_DETECTOR_LED_PIN 4

void setup()
{
  int state = 1;
  double distance;

  // Configure output pins
  pinMode(FAN_MOTOR_PIN, OUTPUT); // FM - fan motor
  pinMode(FAN_LED_PIN, OUTPUT); // FL - fan LED
  pinMode(HAND_DETECTOR_LED_PIN, OUTPUT); // HDL - hand detector LED
  
  while(1)
  {
    // Read inputs
    distance = getDistanceMetres();

    if (state == 1)
    {
      // Control outputs
      digitalWrite(FAN_MOTOR_PIN, LOW); // FM off
      digitalWrite(FAN_LED_PIN, LOW); // FL off
      digitalWrite(HAND_DETECTOR_LED_PIN, LOW); // HDL off
      
      // State transitions
      if (distance < 0.3) state = 2;
    }
    else if (state == 2)
    {s
      // Control outputs
      digitalWrite(FAN_MOTOR_PIN, HIGH); // FM on
      digitalWrite(FAN_LED_PIN, HIGH); // FL on
      digitalWrite(HAND_DETECTOR_LED_PIN, HIGH); // HDL on
      
      // State transitions
      if (distance > 0.5) state = 1;
    }
  }
}

double getDistanceMetres()
{
  // TO BE IMPLEMENTED
}

// loop function will not be used here
void loop()
{
}

//
// State machine example 2
// This version includes measurement of elapsed time in each state
// Written by Ted Burke
// Last updated 24-Nov-2022
//

#define FAN_MOTOR_PIN 2
#define FAN_LED_PIN 5
#define HAND_DETECTOR_LED_PIN 4

int state;
unsigned long state_transition_time; // remember when we entered current state

void setup()
{
  double distance;

  // Configure output pins
  pinMode(FAN_MOTOR_PIN, OUTPUT); // FM - fan motor
  pinMode(FAN_LED_PIN, OUTPUT); // FL - fan LED
  pinMode(HAND_DETECTOR_LED_PIN, OUTPUT); // HDL - hand detector LED

  setState(1);
  
  while(1)
  {
    // Read inputs
    distance = getDistanceMetres();

    if (state == 1)
    {
      // Control outputs
      digitalWrite(FAN_MOTOR_PIN, LOW); // FM off
      digitalWrite(FAN_LED_PIN, LOW); // FL off
      digitalWrite(HAND_DETECTOR_LED_PIN, LOW); // HDL off
      
      // State transitions
      if (distance < 0.3) setState(2);
    }
    else if (state == 2)
    {
      // Control outputs
      digitalWrite(FAN_MOTOR_PIN, HIGH); // FM on
      digitalWrite(FAN_LED_PIN, HIGH); // FL on
      digitalWrite(HAND_DETECTOR_LED_PIN, HIGH); // HDL on
      
      // State transitions
      if (distance > 0.5) setState(3);
    }
    else if (state == 3)
    {
      // Control outputs
      digitalWrite(FAN_MOTOR_PIN, HIGH); // FM on
      digitalWrite(FAN_LED_PIN, HIGH); // FL on
      digitalWrite(HAND_DETECTOR_LED_PIN, LOW); // HDL off
      
      // State transitions
      if (distance < 0.5) setState(2);
      if (millis() - state_transition_time > 5000) setState(1);
    }
  }
}

void setState(int n)
{
  state = n; // update state variable
  state_transition_time = millis(); // remember when the state transition occurred
}

double getDistanceMetres()
{
  // TO BE IMPLEMENTED
}

// loop function will not be used here
void loop()
{
}

//
// State machine example 3
// This version implements each state as a separate while loop nested inside
// one main system loop. 
// Written by Ted Burke
// Last updated 24-Nov-2022
//

#define FAN_MOTOR_PIN 2
#define FAN_LED_PIN 5
#define HAND_DETECTOR_LED_PIN 4

void setup()
{
  double distance;
  int state = 1;
  unsigned long state_transition_time; // remember when we entered current state

  // Configure output pins
  pinMode(FAN_MOTOR_PIN, OUTPUT); // FM - fan motor
  pinMode(FAN_LED_PIN, OUTPUT); // FL - fan LED
  pinMode(HAND_DETECTOR_LED_PIN, OUTPUT); // HDL - hand detector LED

  // main system loop
  while(1)
  {
    while (state == 1)
    {
      // Read inputs
      distance = getDistanceMetres();
      
      // Control outputs
      digitalWrite(FAN_MOTOR_PIN, LOW); // FM off
      digitalWrite(FAN_LED_PIN, LOW); // FL off
      digitalWrite(HAND_DETECTOR_LED_PIN, LOW); // HDL off
      
      // State transitions
      if (distance < 0.3) state = 2;
    }
    
    while (state == 2)
    {
      // Read inputs
      distance = getDistanceMetres();
      
      // Control outputs
      digitalWrite(FAN_MOTOR_PIN, HIGH); // FM on
      digitalWrite(FAN_LED_PIN, HIGH); // FL on
      digitalWrite(HAND_DETECTOR_LED_PIN, HIGH); // HDL on
      
      // State transitions
      if (distance > 0.5) state = 3;
    }

    state_transition_time = millis();
    while (state == 3)
    {
      // Read inputs
      distance = getDistanceMetres();
      
      // Control outputs
      digitalWrite(FAN_MOTOR_PIN, HIGH); // FM on
      digitalWrite(FAN_LED_PIN, HIGH); // FL on
      digitalWrite(HAND_DETECTOR_LED_PIN, LOW); // HDL off
      
      // State transitions
      if (distance < 0.5) state = 2;
      if (millis() - state_transition_time > 5000) state = 1;
    }
  }
}

double getDistanceMetres()
{
  // TO BE IMPLEMENTED
}

// loop function will not be used here
void loop()
{
}

//
// State machine example 4
// Two state machines in parallel
// Written by Ted Burke
// Last updated 24-Nov-2022
//

#define FAN_MOTOR_PIN 2
#define FAN_LED_PIN 5
#define HAND_DETECTOR_LED_PIN 4
#define POWER_LED_PIN 5

int stateA, stateB;
unsigned long stateA_transition_time, stateB_transition_time; // remember when we entered current state

void setup()
{
  double distance;

  // Configure output pins
  pinMode(FAN_MOTOR_PIN, OUTPUT); // FM - fan motor
  pinMode(FAN_LED_PIN, OUTPUT); // FL - fan LED
  pinMode(HAND_DETECTOR_LED_PIN, OUTPUT); // HDL - hand detector LED

  setStateA(1);
  setStateB(1);
  
  while(1)
  {
    // Read inputs
    distance = getDistanceMetres();

    // State machine A: manages the fan motor, fan LED, hadn detection LED
    if (stateA == 1)
    {
      // Control outputs
      digitalWrite(FAN_MOTOR_PIN, LOW); // FM off
      digitalWrite(FAN_LED_PIN, LOW); // FL off
      digitalWrite(HAND_DETECTOR_LED_PIN, LOW); // HDL off
      
      // State transitions
      if (distance < 0.3) setStateA(2);
    }
    else if (stateA == 2)
    {
      // Control outputs
      digitalWrite(FAN_MOTOR_PIN, HIGH); // FM on
      digitalWrite(FAN_LED_PIN, HIGH); // FL on
      digitalWrite(HAND_DETECTOR_LED_PIN, HIGH); // HDL on
      
      // State transitions
      if (distance > 0.5) setStateA(3);
    }
    else if (stateA == 3)
    {
      // Control outputs
      digitalWrite(FAN_MOTOR_PIN, HIGH); // FM on
      digitalWrite(FAN_LED_PIN, HIGH); // FL on
      digitalWrite(HAND_DETECTOR_LED_PIN, LOW); // HDL off
      
      // State transitions
      if (distance < 0.5) setStateA(2);
      if (millis() - stateA_transition_time > 5000) setStateA(1);
    }

    // State machine B: manages the flashing power LED
    if (stateB == 1)
    {
      // Control outputs
      digitalWrite(POWER_LED_PIN, HIGH); // power LED on
      
      // State transitions
      if (millis() - stateB_transition_time >= 500) setStateB(2);
    }
    else if (stateB == 2)
    {
      // Control outputs
      digitalWrite(POWER_LED_PIN, LOW); // power LED off
      
      // State transitions
      if (millis() - stateB_transition_time >= 500) setStateB(1);
    }
  }
}

void setStateA(int n)
{
  stateA = n; // update state variable
  stateA_transition_time = millis(); // remember when the state transition occurred
}

void setStateB(int n)
{
  stateB = n; // update state variable
  stateB_transition_time = millis(); // remember when the state transition occurred
}

double getDistanceMetres()
{
  // TO BE IMPLEMENTED
}

// loop function will not be used here
void loop()
{
}
Posted in Uncategorized | Leave a comment

Example code: two state machines running in parallel

Posted on December 2, 2021 by batchloaf 
//
// State machine example with two parallel state machines.
// State machine A is responsible for motor control.
// State machine B is responsible for LED control.
//

int stateA, stateB;
unsigned long t0_A, t0_B;

void setup()
{
  // Set up pins etc

  // Begin both state machines in state 1
  setStateA(1);
  setStateB(1);

  // Open serial connnection to PC
  Serial.begin(9600);
}

void loop()
{
  int cs;
  
  // Read colour sensor
  cs = analogRead(0);

  // State machine A - motor control
  if (stateA == 1) // pivot forward and left
  {
    // State 1 actions
    setMotors(0, 1); // LM stop, RM forward
    
    // State transition?
    if (cs < 512) setStateA(2);
  }
  else if (stateA == 2) // pivot forward and right
  {
    // State 2 actions
    setMotors(1, 0); // LM forward, RM stop

    // State transition?
    if (cs > 512) setStateA(1);
  }

  // State machine B - LED control
  if (stateB == 1) // LED on
  {
    // State 1 actions
    setLEDs(1); // LED on
    
    // State transition?
    // 0.5s elapsed -> state 2
    if (millis() - t0_B > 500) setStateB(2);
  }
  else if (stateB == 2) // LED off
  {
    // State 2 actions
    setLEDs(0); // LED off

    // State transition?
    // 1.0s elapsed -> state 1
    if (millis() - t0_B > 1000) setStateB(1);
  }  
}

void setStateA(int s)
{
  stateA = s;
  t0_A = millis();

  Serial.print(t0_A);
  Serial.print(" stateA = ");
  Serial.println(stateA);
}

void setStateB(int s)
{
  stateB = s;
  t0_B = millis();

  Serial.print(t0_B);
  Serial.print("stateA = ");
  Serial.println(stateA);
}

void setMotors(int LM_direction, int RM_direction)
{
  digitalWrite(2, LM_direction > 0);
  digitalWrite(3, LM_direction < 0);
  digitalWrite(4, RM_direction > 0);
  digitalWrite(5, RM_direction < 0);
}

void setLEDs(unsigned int current_state)
{
  digitalWrite(6, current_state & 1);
  digitalWrite(7, current_state & 2);
  digitalWrite(8, current_state & 4);
  digitalWrite(9, current_state & 8);
}
Posted in Uncategorized | Leave a comment

State machine examples

Posted on November 25, 2021 by batchloaf

State machine example with helper functions to set motor directions and set LED states.

int state = 1;

void setup()
{
  // Set up pins etc.
}

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

  // Implement the actions for the current state
  if (state == 1)
  {
    // State 1 actions
    setMotors(1, 1); // both motors forwards
    setLEDs(state);
    
    // State transition?
    if (frs > 512) state = 2;
    if (fls > 512) state = 3;
  }
  else if (state == 2) // frs hits white first
  {
    // State 2 actions
    setMotors(1, 0); // LM forward, RM stop
    setLEDs(state);

    // State transition?
    if (fls > 512) state = 4;
  }
  else if (state == 3) // fls hits white first
  {
    // State 3 actions
    setMotors(0, 1); // LM stop, RM forward
    setLEDs(state);

    // State transition?
    if (frs > 512) state = 4;
  }
  else if (state == 4)
  {
    // State 4 actions
    setMotors(-1, -1); // both motors reverse
    setLEDs(state);

    // State transition?
    if (bls > 512 || brs > 512) state = 1;
  }
}

void setMotors(int LM_direction, int RM_direction)
{
  digitalWrite(2, LM_direction > 0);
  digitalWrite(3, LM_direction < 0);
  digitalWrite(4, RM_direction > 0);
  digitalWrite(5, RM_direction < 0);
}

void setLEDs(int current_state)
{
  digitalWrite(6, current_state == 1);
  digitalWrite(7, current_state == 2);
  digitalWrite(8, current_state == 3);
  digitalWrite(9, current_state == 4);
}

State machine example with state timer and function to change state.

int state;
unsigned long stateEnteredTime;

void setup()
{
  // Set up pins etc

  // Begin in state 1
  setState(1);
}

void loop()
{
  int fls, frs, bls, brs;
  float d;
  
  // Read all sensors
  fls = analogRead(0);
  frs = analogRead(1);
  bls = analogRead(2);
  brs = analogRead(3);
  d = getDistance();

  // Implement the actions for the current state
  if (state == 1) // spin / search
  {
    // State 1 actions
    setMotors(1, -1); // LM forward, RM reverse
    setLEDs(state);
    
    // State transition?
    if (d < 0.7) setState(2);
  }
  else if (state == 2) // pause
  {
    // State 2 actions
    setMotors(0, 0); // LM stop, RM stop
    setLEDs(state);

    // State transition?
    // 1s elapsed -> state 3
    if (millis() - stateEnteredTime > 1000) setState(3);
    if (d > 0.7) setState(1);
  }
  else if (state == 3) // attack
  {
    // State 3 actions
    setMotors(1, 1); // LM forward, RM forward
    setLEDs(state);

    // State transition?
    if (d > 0.7) setState(1);
    if (fls > 512) setState(4);
    if (frs > 512) setState(5);
  }
  else if (state == 4) // straighten up right
  {
    // State 4 actions
    setMotors(0, 1); // LM stop, RM forward
    setLEDs(state);

    // State transition?
    if (frs > 512) setState(6);
  }
  else if (state == 5) // straighten up left
  {
    // State 5 actions
    setMotors(1, 0); // LM forward, RM stop
    setLEDs(state);

    // State transition?
    if (fls > 512) setState(6);
  }
  else if (state == 6) // reverse
  {
    // State 6 actions
    setMotors(-1, -1); // LM reverse, RM reverse
    setLEDs(state);

    // State transition?
    // 2s elapsed -> state 1
    if (millis() - stateEnteredTime > 2000) setState(1);
    if (d < 0.7) setState(3);
  }
}

void setState(int s)
{
  state = s;
  stateEnteredTime = millis();
}

void setMotors(int LM_direction, int RM_direction)
{
  digitalWrite(2, LM_direction > 0);
  digitalWrite(3, LM_direction < 0);
  digitalWrite(4, RM_direction > 0);
  digitalWrite(5, RM_direction < 0);
}

void setLEDs(unsigned int current_state)
{
  digitalWrite(6, current_state & 1);
  digitalWrite(7, current_state & 2);
  digitalWrite(8, current_state & 4);
  digitalWrite(9, current_state & 8);
}
Posted in Uncategorized | Leave a comment

Dual HC-SR04 rangefinder example for Arduino Nano

Posted on November 17, 2021 by batchloaf 
//
// Dual HC-SR04 rangefinder example
// Takes distance readings from two HC-SR04
// ultrasonic rangefinders and plots them as
// left-right pairs in the Serial Plotter.
//
// Written by Ted Burke - last updated 17-Nov-2021
//

void setup()
{
  pinMode(3, OUTPUT); // right sensor trigger pin
  pinMode(4, INPUT);  // right sensor echo pin
  
  pinMode(5, OUTPUT); // left sensor trigger pin
  pinMode(6, INPUT);  // left sensor echo pin
  
  Serial.begin(9600); // open serial connection to PC
}

void loop()
{
  float dleft, dright;     // left and right distance
  dleft  = getDistance(1); // read left sensor
  dright = getDistance(2); // read right sensor

  // Limit the distance measurements to 1.0m maximum
  if (dleft > 1.0) dleft = 1.0;
  if (dright > 1.0) dright = 1.0;

  // The graph scaling factor is just to magnify the
  // size of the graph in the Arduino Serial Plotter
  float graph_scaling_factor = 5.0;
  Serial.print(graph_scaling_factor * dleft);
  Serial.print(" ");
  Serial.println(graph_scaling_factor * -dright);
}

float getDistance(int left_or_right)
{
  // Declare variables
  int trig_pin, echo_pin;
  long echo_duration_us;
  float distance_m;

  // Delay to allow ultrasound echoes to dissipate
  delay(50);
  
  // Use trig and echo pins for specified sensor
  if (left_or_right == 1)
  {
    // left sensor selected
    trig_pin = 5;
    echo_pin = 6;
  }
  else
  {
    // right sensor selected
    trig_pin = 3;
    echo_pin = 4;
  }
  
  // Send 20us trigger pulse
  digitalWrite(trig_pin, HIGH); // begin pulse
  delayMicroseconds(20); // 20 microsecond delay
  digitalWrite(trig_pin, LOW);  // end pulse

  // Measure the duration of the echo pulse
  echo_duration_us = pulseIn(echo_pin, HIGH);

  // Calculate distance in metres
  distance_m = 340e-6 * echo_duration_us * 0.5;

  // return the measured distance to the calling function
  return distance_m;
}

Example output in Serial Plotter:

Posted in Uncategorized | Leave a comment

Mini project brainstorming

Posted on November 11, 2021 by batchloaf
Posted in Uncategorized | Leave a comment

State machine example from today’s class

Posted on November 11, 2021 by batchloaf 
int state = 1; // state variable

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

  Serial.begin(9600);
}

void loop()
{
  int LS, MS, RS; // left, middle and right sensor readings

  // Read all sensors
  LS = analogRead(0);
  MS = analogRead(1);
  RS = analogRead(2);

  Serial.println(state);
  
  if (state == 1)
  { 
    // actuators
    digitalWrite(2, LOW);  // LM stop
    digitalWrite(3, LOW);
    digitalWrite(4, HIGH); // RM forward
    digitalWrite(5, LOW);

    // state transitions
    if (MS > 500) state = 2;
    if (LS < 500) state = 3;
    if (RS < 500) state = 3;
  }
  else if (state == 2)
  {
    // actuators
    digitalWrite(2, HIGH); // LM forward
    digitalWrite(3, LOW);
    digitalWrite(4, LOW);  // RM stop
    digitalWrite(5, LOW);

    // state transitions
    if (MS < 500) state = 1;
    if (LS < 500) state = 3;
    if (RS < 500) state = 3;
  }
  else if (state == 3)
  {
    // actuators
    digitalWrite(2, LOW);  // LM stop
    digitalWrite(3, LOW);
    digitalWrite(4, LOW);  // RM stop
    digitalWrite(5, LOW);

    delay(2000);
    state = 4;
  }
  else if (state == 4)
  {
    // actuators
    digitalWrite(2, HIGH);  // LM forward
    digitalWrite(3, LOW);
    digitalWrite(4, HIGH); // RM forward
    digitalWrite(5, LOW);

    delay(1000);
    state = 1;
  }
}
Posted in Uncategorized | Leave a comment

State machine example from lecture on 12 Nov 2020

Posted on November 12, 2020 by batchloaf
State table for this example

This is the code from the state machine example:

//
// State table example
// Written by Ted Burke
// Last updated 12 Nov 2020
//

int state = 1;

void setup()
{
  pinMode(4, OUTPUT); // digital output for green LED
  pinMode(5, OUTPUT); // digital output for red LED

  pinMode(2, OUTPUT); // digital output for HC-SR04 trigger 
  pinMode(3, INPUT);  // digital input for HC-SR04 echo

  Serial.begin(9600); // open serial connection to PC at 9600 bits/second
}

void loop()
{
  // Declare sensor variables
  float d;
  int v, s;
  
  // Read all inputs 
  d = distance();      // read distance in metres from HC-SR04 rangefinder
  v = analogRead(0);   // read voltage (0-1023 du) from TCRT5000 IR sensor
  s = digitalRead(13); // read the microswitch (1 = pressed, 0 = not pressed)

  // Print sensor readings on serial monitor
  Serial.print(d);
  Serial.print(" ");
  Serial.print(v);
  Serial.print(" ");
  Serial.println(s);

  // Implement the state machine
  if (state == 1)
  {
    // set outputs
    digitalWrite(4, LOW); // green LED off
    digitalWrite(5, LOW); // red LED off

    // state transitions
    if (v > 512) state = 2;
    if (d < 0.2) state = 3;
  }
  else if (state == 2)
  {
    // set outputs
    digitalWrite(4, HIGH); // green LED on
    digitalWrite(5, LOW);  // red LED off

    // state transitions
    if (v <= 512) state = 1;
    if (d < 0.2) state = 4;
  }
  else if (state == 3)
  {
    // set outputs
    digitalWrite(4, LOW);  // green LED off
    digitalWrite(5, HIGH); // red LED on

    // state transitions
    if (v > 512) state = 4;
    if (d >= 0.2) state = 1;
  }
  else if (state == 4)
  {
    // set outputs
    digitalWrite(4, HIGH); // green LED on
    digitalWrite(5, HIGH); // red LED on

    // state transitions
    if (s == 1) state = 1;
  }
}

// Returns the rangefinder distance in metres
float distance()
{
  // Send a trigger pulse to the rangefinder
  digitalWrite(2, HIGH);
  delayMicroseconds(20);
  digitalWrite(2, LOW);
 
  // Measure echo pulse duration, then calculate and return distance
  return (pulseIn(3, HIGH, 60000) * 340e-6 / 2);
}

This is the second version of the example code with state time included:

//
// State table example
// Written by Ted Burke
// Last updated 12 Nov 2020
//

int state = 1;
unsigned long stateStartTime;

void setup()
{
  pinMode(4, OUTPUT); // digital output for green LED
  pinMode(5, OUTPUT); // digital output for red LED

  pinMode(2, OUTPUT); // digital output for HC-SR04 trigger 
  pinMode(3, INPUT);  // digital input for HC-SR04 echo

  Serial.begin(9600); // open serial connection to PC at 9600 bits/second
}

void loop()
{
  // Declare sensor variables
  float d;
  int v, s;
  
  // Read all inputs 
  d = distance();      // read distance in metres from HC-SR04 rangefinder
  v = analogRead(0);   // read voltage (0-1023 du) from TCRT5000 IR sensor
  s = digitalRead(13); // read the microswitch (1 = pressed, 0 = not pressed)

  // Print sensor readings on serial monitor
//  Serial.print(state);
//  Serial.print(" ");
//  Serial.print(d);
//  Serial.print(" ");
//  Serial.print(v);
//  Serial.print(" ");
//  Serial.println(s);

  // Implement the state machine
  if (state == 1)
  {    
    // set outputs
    digitalWrite(4, LOW); // green LED off
    digitalWrite(5, LOW); // red LED off

    // state transitions
    if (v > 512) changeState(2);
    if (d < 0.2) changeState(3);
  }
  else if (state == 2)
  {
    // set outputs
    digitalWrite(4, HIGH); // green LED on
    digitalWrite(5, LOW);  // red LED off

    // state transitions
    if (v <= 512) changeState(1);
    if (d < 0.2) changeState(4);
  }
  else if (state == 3)
  {
    // set outputs
    digitalWrite(4, LOW);  // green LED off
    digitalWrite(5, HIGH); // red LED on

    // state transitions
    if (v > 512) changeState(4);
    if (d >= 0.2) changeState(1);
  }
  else if (state == 4)
  {
    // set outputs
    digitalWrite(4, HIGH); // green LED on
    digitalWrite(5, HIGH); // red LED on

    // state transitions
    if (s == 1) changeState(1);
    if (millis() - stateStartTime > 5000) changeState(1);
  }
}

void changeState(int n)
{
  state = n; // Update the state variable

  Serial.println(state);

  // Remember what time we entered this state (in ms since the Arduino turned on)
  stateStartTime = millis();
}

// Returns the rangefinder distance in metres
float distance()
{
  // Send a trigger pulse to the rangefinder
  digitalWrite(2, HIGH);
  delayMicroseconds(20);
  digitalWrite(2, LOW);
 
  // Measure echo pulse duration, then calculate and return distance
  return (pulseIn(3, HIGH, 60000) * 340e-6 / 2);

  // Short delay to allow ultrasound echoes to dissipate
  delay(50);
}

Here’s a photo of the state machine example circuit:

Here’s a video of a SCARA robot arm:

Posted in Uncategorized | Leave a comment

State machine example from class – “Shoo Robot”

Posted on November 14, 2019 by batchloaf 

//
// 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);
  }
}
Posted in Uncategorized | Leave a comment

State machine example – Golf ball collector

Posted on November 22, 2018 by batchloaf

State table whiteboard snapshots:

Code:

//
// State table example for Arduino Nano - Golf Ball Collector
// Written by Ted Burke - last updated 22-11-2018
//
 
int state = 1;
int step_delay = 10;
 
// Remember which winding is active on each stepper
int active_winding_left = 1;
int active_winding_right = 1;
 
// Function prototypes
void left_step_forward();
void right_step_forward();
double distance_front();
double distance_left();
double distance_right();
 
void setup()
{
    // Actuator control pins
    pinMode(2, OUTPUT); // LM winding 1
    pinMode(3, OUTPUT); // LM winding 2
    pinMode(4, OUTPUT); // LM winding 3
    pinMode(5, OUTPUT); // LM winding 4
     
    pinMode(6, OUTPUT); // RM winding 1
    pinMode(7, OUTPUT); // RM winding 2
    pinMode(8, OUTPUT); // RM winding 3
    pinMode(9, OUTPUT); // RM winding 4
     
    // Sensor pins
    pinMode(10, OUTPUT); // trigger pin for front rangefinder
    pinMode(11, INPUT);  // echo pin for front rangefinder
     
    //
    // FILL IN PINS FOR LEFT AND RIGHT RANGEFINDERS
    //
}
 
void loop()
{
    int steps;
    int rff, rfl, rfr;
     
    while(state == 1) // Forward (up the field)
    {
        // Actuators
        left_step_forward();
        right_step_forward();
         
        // Read sensors
        rff = distance_front();
        rfl = distance_left();
        rfr = distance_right();
         
        // Change state?
        if (rff < 2.0 && rfr > 1.5) state = 2;
        if (rff < 2.0 && rfr <= 1.5) state = 4;
    }
     
    steps = 0;
    while(state == 2) // Turn right
    {
        // Actuators
        left_step_forward(); // left motor forward, right motor stop
        delay(step_delay);
         
        // Count steps
        steps = steps + 1;
         
        // Change state?
        if (steps == 805) state = 3;
    }
     
    while(state == 3) // Return (down the field)
    {
        // Actuators
        left_step_forward();
        right_step_forward();
         
        // Read sensors
        rff = distance_front();
        rfl = distance_left();
        rfr = distance_right();
         
        // Change state?
        if (rff < 2.0 && rfl > 1.5) state = 4;
        if (rff < 2.0 && rfl <= 1.5) state = 2;
    }
     
    steps = 0;
    while(state == 4) // Turn left
    {
        // Actuators
        right_step_forward(); // left motor forward, right motor stop
        delay(step_delay);
         
        // Count steps
        steps = steps + 1;
         
        // Change state?
        if (steps == 805) state = 1;
    }   
}
 
void set_left_windings(int a, int b, int c, int d)
{
    digitalWrite(2, a);
    digitalWrite(3, b);
    digitalWrite(4, c);
    digitalWrite(5, d);
}
 
void left_step_forward()
{
    // Select next winding
    active_winding_left = active_winding_left + 1;
    if (active_winding_left == 5) active_winding_left = 1;
     
    // Set the state of all four windings
    if (active_winding_left == 1) set_left_windings(1, 0, 0, 0);
    if (active_winding_left == 2) set_left_windings(0, 1, 0, 0);
    if (active_winding_left == 3) set_left_windings(0, 0, 1, 0);
    if (active_winding_left == 4) set_left_windings(0, 0, 0, 1);
}
 
void set_right_windings(int a, int b, int c, int d)
{
    digitalWrite(6, a);
    digitalWrite(7, b);
    digitalWrite(8, c);
    digitalWrite(9, d);
}
 
void right_step_forward()
{
    // Select next winding
    active_winding_right = active_winding_right + 1;
    if (active_winding_right == 5) active_winding_right = 1;
     
    // Set the state of all four windings
    if (active_winding_right == 1) set_right_windings(1, 0, 0, 0);
    if (active_winding_right == 2) set_right_windings(0, 1, 0, 0);
    if (active_winding_right == 3) set_right_windings(0, 0, 1, 0);
    if (active_winding_right == 4) set_right_windings(0, 0, 0, 1);
}
 
double distance_front()
{
    // Declare variables
    double d;
    unsigned long t1, t2, duration;
     
    // Trigger pulse to front rangefinder
    digitalWrite(10, HIGH);
    delayMicroseconds(20);
    digitalWrite(10, LOW);
     
    // Measure the duration of the echo pulse
    while(digitalRead(11) == 0); // Wait for start of echo pulse
    t1 = micros();               // Remember start time of pulse
    while(digitalRead(11) == 1); // Wait while echo remains high
    t2 = micros();
    duration = t2 - t1;
     
    // Convert duration into distance in metres
    d = (duration * 1e-6 * 340.0) / 2.0;
     
    return d;
}
 
double distance_left()
{
    // Fill these in yourself
}
 
double distance_right()
{
    // Fill these in yourself
}
Posted in Uncategorized | Leave a comment