#include "fsm.h"
#include "timeout.h"

typedef EFSM<Timeout> timeFSM;


// Button handling.
void startState(FSM& fsm) {
  Serial.println("Press button A to start.");
  fsm.newState(waitState);
}

void waitState(FSM& fsm) {
  if (digitalRead(2) == LOW) {
    Serial.println("Entering run state, press B to restart, press C to stop.");
    fsm.newState(runState);
  }
}

void runState(FSM& fsm) {
  if (digitalRead(3) == LOW) {
    Serial.println("Restarting.");
    fsm.newState(startState);
  }
  if (digitalRead(4) == LOW) {
    Serial.println("Entering stop state.");
    fsm.newState(stopState);
  }
}

void stopState(FSM&) {}


// LED handling.
void ledOn(timeFSM& fsm) {
  if (fsm.data.timeout()) {
    digitalWrite(LED_BUILTIN, HIGH);
    fsm.data.reset();
    fsm.newState(ledOff);
  }
}

void ledOff(timeFSM& fsm) {
  if (fsm.data.timeout()) {
    digitalWrite(LED_BUILTIN, LOW);
    fsm.data.reset();
    fsm.newState(ledOn);
  }
}


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

  for (size_t i = 2; i <= 4; i++) {
    pinMode(i, INPUT_PULLUP);
  }
  pinMode(LED_BUILTIN, OUTPUT);
}

void loop() {
  static Timeout timeout(600);
  static timeFSM ledFsm(ledOn, timeout);
  ledFsm.loop();

  static FSM btnFsm(startState);
  btnFsm.loop();
}

sketch.ino

diagram.json

#pragma once

template <class T>
class EFSM {
public:
  EFSM(void (* const)(EFSM<void*>&));
  EFSM(void (* const)(EFSM<T>&), T& data);

  void loop();
  void newState(void (* const)(EFSM<T>&));

  T& data;

private:
  void (*state_)(EFSM<T>&){};
  static void* default_;
};

typedef EFSM<void*> FSM;


template <class T>
EFSM<T>::EFSM(void (* const state)(EFSM<void*>&)) : data(default_) {
  newState(state);
}

template <class T>
EFSM<T>::EFSM(void (* const state)(EFSM<T>&), T& data) : data(data) {
  newState(state);
}

template <class T>
void EFSM<T>::loop() {
  state_(*this);
}

template <class T>
void EFSM<T>::newState(void (* const state)(EFSM<T>&)) {
  state_ = state;
}

template <class T>
void* EFSM<T>::default_ = nullptr;

fsm.h

#pragma once

class Timeout {
public:
  Timeout() {};
  Timeout(unsigned long const);

  void set(unsigned long const);
  void reset();
  bool timeout() const;

private:
  unsigned long start_;
  unsigned long timeout_;
};

timeout.h

#include <Arduino.h>

#include "timeout.h"


Timeout::Timeout(unsigned long const timeout) {
  set(timeout);
}

void Timeout::set(unsigned long const timeout) {
  timeout_ = timeout;
  reset();
}

void Timeout::reset() {
  start_ = millis();
}

bool Timeout::timeout() const {
  return millis() - start_ >= timeout_;
}