#include "StateMachine.h"
#include "Timer.h"
#include "Sensor.h"
#include "Drive.h"

// Connections
#define LEFT_SERVO_PIN 10
#define RIGHT_SERVO_PIN 11

// Define state machine actions
const int NUL = bit(0);
const int PST = bit(1);
const int STS = bit(2);
const int LTS = bit(3);

// Define state machine events
const int TRU = bit(0);
const int STA = bit(1);
const int LTA = bit(2);

// Create objects 
StateMachine mm;
Timer shortTimer(100);
Timer longTimer(500);
Drive myDrive;

void setup() {
  Serial.begin(9600);

  myDrive.attach(LEFT_SERVO_PIN, RIGHT_SERVO_PIN);
  myDrive.forward(0.5);
   
  // Construct state machine
  mm.addState(0, TRU, 1, STS + PST, 0, NUL);
  mm.addState(1, STA, 2, LTS + PST, 1, NUL);
  mm.addState(2, LTA, 1, STS + PST, 2, NUL);
}

void loop() {
  // Log events
  mm.logEvent(TRU, true);
  mm.logEvent(STA, shortTimer.isAlarming());
  mm.logEvent(LTA, longTimer.isAlarming());
  
  // Evaluate state machine
  mm.evaluate();
  
  // Perform requested actions
  if(mm.requests(PST))
  {
    Serial.print("New state: ");
    Serial.println(mm.getState());
  }
  if(mm.requests(STS)) shortTimer.reset();
  if(mm.requests(LTS)) longTimer.reset();
}


sketch.ino

diagram.json

#include "StateMachine.h"

#define STOPPED  0
#define STARTING 1
#define RUNNING  2
#define STOPPING 3

StateMachine::StateMachine() {}

void StateMachine::addState(int _state, int _trigger, int _trueDestination, int _trueActions, int _falseDestination, int _falseActions)
{
  triggers[_state] = _trigger;
  trueActions[_state] = _trueActions;
  falseActions[_state] = _falseActions;
  trueDestinations[_state] = _trueDestination;
  falseDestinations[_state] = _falseDestination;
}

void StateMachine::onStart(int _startActions)
{
  startActions = _startActions;
}

void StateMachine::onStop(int _stopActions)
{
  stopActions = _stopActions;
}

int StateMachine::createEventHandle()
{
  eventCount ++;
  return bit(eventCount - 1);
}

int StateMachine::createActionHandle()
{
  actionCount ++;
  return bit(actionCount - 1);
}

void StateMachine::logEvent(int _event, bool _status)
{
  if (_status) currentEvents = currentEvents | _event;
}

void StateMachine::evaluate()
{
  switch(status)
  {
    case STARTING:
      Serial.println("Starting");
      currentActions = startActions;
      currentState = 0;
      status=RUNNING;
      break;
    case RUNNING:
      Serial.println(currentState);
      if ((triggers[currentState] & currentEvents) > 0)
      {
        currentActions = trueActions[currentState];
        currentState = trueDestinations[currentState];
      }
      else
      {
        currentActions = falseActions[currentState];
        currentState = falseDestinations[currentState];
      }
      break;
    case STOPPING:
      Serial.println("Stopping");
      status = STOPPED;
      currentActions = stopActions;
      currentState = 0;
      status=STOPPED;
      break;
    case STOPPED:
      // Serial.println("Stopped");
      currentActions = 0;
      currentState = 0;
      break;
  }
  currentEvents = 0;
}

void StateMachine::start()
{
  Serial.println("Told to start.");
  status = STARTING;
}

void StateMachine::stop()
{
  Serial.println("Told to stop.");
  status = STOPPING;
}

int StateMachine::getState()
{
  return currentState;
}

bool StateMachine::requests(int _action)
{
  return (currentActions & _action) > 0;
}

StateMachine.ino

#pragma once

#define MAX_STATES 20

class StateMachine
{
  private:
    int status = 0;

    int triggers[MAX_STATES];
    int trueActions[MAX_STATES];
    int falseActions[MAX_STATES];
    int trueDestinations[MAX_STATES];
    int falseDestinations[MAX_STATES];

  	int currentState;
    int currentEvents;
  	int currentActions;
    int startActions;
    int stopActions;

    int eventCount;
    int actionCount;
  
  public:
  	StateMachine();
    void addState(int _state, int _trigger, int _trueDestination, int _trueActions, int _falseDestination,  int _falseActions);
    void onStart(int _startActions);
    void onStop(int _stopActions);
    int createEventHandle();
    int createActionHandle(); 
    void logEvent(int _event, bool _status);
    void evaluate();
    void start();
    void stop();
    int getState();
    bool requests(int _action);
};

StateMachine.h

#pragma once

class Timer
{
  private:
    int targetTime;
    int duration;
  public:
    Timer(int _duration);
    void reset();
    bool isAlarming();
};

Timer.h

#include "Timer.h"

Timer::Timer(int _duration) 
{
  duration = _duration;
}

void Timer::reset()
{
  targetTime = millis() + duration;
  Serial.print("Setting timer for ");
  Serial.print(duration);
  Serial.println(" ms");     
}

bool Timer::isAlarming()
{
  return millis() >= targetTime;
}

Timer.ino

#include "Sensor.h"
  
int Sensor::countEvents()
{
  int eventCount = 0;
  unsigned long timeMarker = millis();
  while(millis()-timeMarker < mySamplePeriod) {
    if (digitalRead(myPin) == LOW) 
    {
      eventCount ++;
    }
  }
  return eventCount;
}

Sensor::Sensor(byte _pin, int _samplePeriod, int _sampleThreshold) 
{
  myPin = _pin;
  mySamplePeriod = _samplePeriod;
  mySampleThreshold = _sampleThreshold;
  
  pinMode(myPin, INPUT);
}

bool Sensor::alarm()
{
  return countEvents() >= mySampleThreshold;
}

Sensor.ino

#pragma once

class Sensor
{
  private:
    byte myPin;
  	int mySamplePeriod;
  	int mySampleThreshold;
  	int countEvents();
    
  public:
    Sensor(byte _pin, int _samplePeriod, int _sampleThreshold);  
    bool alarm();   
};

Sensor.h

#include "Drive.h"
#include <Servo.h>

#define SERVO_SPEED 90
#define SERVO_STOP 91


Drive::Drive() {}

void Drive::attach(int _leftPin, int _rightPin)
{
  rightServo.attach(_rightPin);
  leftServo.attach(_leftPin);
}
void Drive::forward(float _speed)
{
  if (_speed > 1.0) _speed = 1.0;

  int rightServoFwd = SERVO_STOP - SERVO_SPEED * _speed;
  int leftServoFwd = SERVO_STOP + SERVO_SPEED * _speed;

  rightServo.write(rightServoFwd);
  leftServo.write(leftServoFwd);
}

void Drive::reverse(float _speed)
{
  if (_speed > 1.0) _speed = 1.0;

  int rightServoRev = SERVO_STOP + SERVO_SPEED ;
  int leftServoRev = SERVO_STOP - SERVO_SPEED ;

  rightServo.write(rightServoRev);
  leftServo.write(leftServoRev);
}

void Drive::left(float _speed)
{
  if (_speed > 1.0) _speed = 1.0;
  if (_speed < 0.0) _speed = 0.0;

  rightServo.write(SERVO_STOP - SERVO_SPEED * _speed);
  leftServo.write(SERVO_STOP - SERVO_SPEED * _speed);
}

void Drive::right(float _speed)
{
  if (_speed > 1.0) _speed = 1.0;

  int rightServoRev = SERVO_STOP + SERVO_SPEED ;
  int leftServoFwd = SERVO_STOP + SERVO_SPEED ;

  rightServo.write(rightServoRev);
  leftServo.write(leftServoFwd);
}

void Drive::stop()
{
  rightServo.write(SERVO_STOP);
  leftServo.write(SERVO_STOP);
}

Drive.ino

# Wokwi Library List
# See https://docs.wokwi.com/guides/libraries

Servo

libraries.txt

#pragma once
#include <Servo.h>

class Drive
{
  private:
    Servo rightServo;
    Servo leftServo;

  public:
    Drive();
    void attach(int _leftPin, int _rightPin);
    void forward(float _speed);
    void reverse(float _speed);
    void left(float _speed);
    void right(float _speed);
    void stop();
};