This script was used in our student project in 2019 to controll a car to autonomously follow a line of a specific colour. A Raspberry Pi was used to capture images and analyse them to pass the image information via a serial connection to the arduino. The arduino used this information to controll the car.

finite_state_machine_arduino_drive_car.c

/**
Script Name: finite_state_machine_arduino_drive_car.c
Author: Stefan Herdy
Date: 15.12.2019
Description: 
This script was used in our student project in 2019 to controll a car to autonomously follow a line of a specific colour. 
A Raspberry Pi was used to capture images and analyse them to pass the image information via a serial connection to the arduino. 
The arduino used this information to controll the car.

Usage: 
- Modify the script to our requirements and use it for your Ardiuno project or just watch how a finite state machin can be implemented in C++.
**/
  
#include <Servo.h>


//Define variables

//Driving system
Servo drive;
Servo control;
int driveValue = 0;
int steerValue = 0;
int button = 3;           // Pin 3 (O5)
int led = 10;             // Pin 10 (O1)
int sound = 7;
int trigger = 9;
int echo = 8;
long duration = 0;
long distance = 20;
int stopDistance = 40;
int startDistance = 50 ;
int t = 0;
int tmax = 100;
int b = 0;

//Variables for the serial protocol
const byte numChars = 32;
char receivedChars[numChars];
boolean newData = false;

//States
enum enSTATES
  {
    INIT,
    STANDBY,
    DRIVE,
    STOP,
    OBSTACLE,
  } STATE;

void setup()
{
  Serial.begin(115200);
  
  drive.attach(11);             //Pin 11 (O0)
  control.attach(5);            //Pin 5 (O4)
  
  pinMode(trigger, OUTPUT);
  pinMode(echo, INPUT); 
  pinMode(button, INPUT);
  pinMode(led, OUTPUT);
  pinMode(sound, INPUT);
  
  STATE = INIT;
}


void loop()
{
  // The driveValues were only valid for our setup and are depending on the used drive controller
  t=t+1;
  switch (STATE)
  {
    case INIT:

    //Set the speed 0
    driveValue = 1550;
    delay(50);
    if (t > tmax)
    {
      STATE = STANDBY;
      t=0;
    }
    break;


    case STANDBY:

    driveValue = 1550;
    serialProtocol();
    steer();
    digitalWrite(led, LOW);
    if (digitalRead(button) == HIGH)
    {
      delay(50);
      STATE = DRIVE;
    }
    break;


    case DRIVE:

    driveValue = 1624 ;
    serialProtocol();
    steer();
    if (digitalRead(button) == HIGH)
    {
      delay(50);
      STATE = STANDBY;
    }
    if (digitalRead(sound) == HIGH)
    {
      delay(50);
      brake();
      STATE = STOP;
    }
    getDistance();
    if (distance <= stopDistance)
    {
      brake();
      STATE = OBSTACLE;
    }
    break;


    case STOP:

    getDistance();
    if (distance <= stopDistance)
    {
      digitalWrite(led, HIGH);
    }
    if (distance >= 30)
    {
      digitalWrite(led, LOW);
    }
    if (digitalRead(sound) == HIGH && distance >= startDistance)
    {
      digitalWrite(led, LOW);
      delay(50);
      STATE = DRIVE;
    }
    if (digitalRead(button) == HIGH)
    {
      delay(50);
      STATE = STANDBY;
    }
    break;


    case OBSTACLE:

    digitalWrite(led, HIGH);
    getDistance();
    if (distance >= startDistance)
    {
      digitalWrite(led, LOW);
      delay(50);
      STATE = DRIVE;
    }
    if (digitalRead(sound) == HIGH)
    {
      delay(50);
      STATE = STOP;
    }
    if (digitalRead(button) == HIGH)
    {
      delay(50);
      STATE = STANDBY;
    }
    break;
  }


   drive.writeMicroseconds(driveValue);      //den Wert an den Servomotor senden
   control.write(steerValue);                // den Wert an die Steuerung senden
}


void serialProtocol()
{
  static boolean recvInProgress = false;
  static byte ndx = 0;
  char startMarker = '<';
  char endMarker = '>';
  char rc;
 
  while (Serial.available() > 0 && newData == false)
  {
    rc = Serial.read();

    if (recvInProgress == true)
    {
      if (rc != endMarker)
      {
        receivedChars[ndx] = rc;
        ndx++;
        if (ndx >= numChars)
        {
          ndx = numChars - 1;
        }
      }
         
      else
      {
        receivedChars[ndx] = '\0';            // terminate the string
        recvInProgress = false;
        ndx = 0;
        newData = true;
      }
    }

    else if (rc == startMarker)
    {
      recvInProgress = true;
    }
  }
}


void steer()
{
  if (newData == true)
  {
    newData = false;
    steerValue = atoi(receivedChars);
  }
}


void getDistance()
{
  // Clear trigger
  digitalWrite(trigger, LOW);
  delayMicroseconds(2);

  // Send wave
  digitalWrite(trigger, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigger, LOW);
  
  // Compute distance in cm out of the wave travel duration
  duration = pulseIn(echo, HIGH);
  distance = duration / (2*29.1);
}


void brake()
{
  driveValue = 800;
  drive.writeMicroseconds(driveValue);
  delay(250);
  driveValue = 1550;
  drive.writeMicroseconds(driveValue);
}