#include "fsm.h"


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&) {}


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

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

void loop() {
  static FSM fsm(startState);

  fsm.loop();
}

sketch.ino

diagram.json

#pragma once

class FSM {
public:
  FSM(void (* const)(FSM&));

  void loop();
  void newState(void (* const)(FSM&));

private:
  void (*state_)(FSM&){};
};

fsm.h

#include "fsm.h"


FSM::FSM(void (* const state)(FSM&)) {
  newState(state);
}

void FSM::loop() {
  state_(*this);
}

void FSM::newState(void (* const state)(FSM&)) {
  state_ = state;
}