#pragma once
#include "msg.h"
#include "cmd.h"
void setup() {
  Serial.begin(115200);
  Serial.println("...");
  CMD::init();
  //MSG::begin();
}

void loop() {
  //MSG::loop();
  CMD::next();
}


sketch.ino

diagram.json

#include <Arduino.h>
#include <EEPROM.h>
#include "env.h"
namespace ENV {
  byte CHOKE_ON = 0;
  byte CHOKE_OFF = 0;
  void init() {
    bool configured = (EEPROM.read(0) == 42);
    CHOKE_OFF = configured ? EEPROM.read(1) : 0;
    CHOKE_ON = configured ? EEPROM.read(2) : 0;
  }

  void save( byte val, char* state ) {
    EEPROM.update(0, 42);
    if (state == "off") {
      EEPROM.write(1, val);
      CHOKE_OFF = val;
    } else if (state == "on") {
      EEPROM.write(2, val);
      CHOKE_ON = val;
    }
  }
};



env.cpp

#include "cmd.h"
#include "avr.h"
#include "sensor.cpp"

namespace CMD {
  Servo servo;
  byte CHOKE_OFF = 0;
  byte CHOKE_ON = 0;
  DHT dht(ENV::DHT_PIN, DHT22, 6);
  char message[ENV::CMD_MAX_LENGTH];
  uint8_t i = 0;

  void on_city_change(uint8_t value) { 
    if (value == HIGH) A::trigger(A::EVT::STOP); 
  }
  Sensor city(ENV::CITY_DEBOUNCE_MS, ENV::CITY_PORT, on_city_change);

  void on_gen_change(uint8_t value) { 
    A::trigger(value == HIGH ? A::EVT::ENGINE_ON : A::EVT::ENGINE_OFF); 
  }
  Sensor gen(ENV::GEN_DEBOUNCE_MS, ENV::GEN_PORT, on_gen_change);

  void save( byte val, enum SERVO_STATES state ) {
    EEPROM.write(0, 42);
    if (state ==  SERVO_STATES::OFF) {
      EEPROM.write(1, val);
      CHOKE_OFF = val;
    } else if (state == SERVO_STATES::ON) {
      EEPROM.write(2, val);
      CHOKE_ON = val;
    }
  }

  void setServo(byte pos, enum SERVO_STATES state) {
    servo.write(pos); 
    save(pos, state);
  }

  void getServo(enum SERVO_STATES state) {
    byte pos = state == SERVO_STATES::OFF ? CHOKE_OFF : CHOKE_ON;
    servo.write(pos); 
  }

  void(* rst) (void) = 0;

  void scan() {
    unsigned long timer = (millis() - A::fsm.triggered) / 1000;
    char m[40];
    uint8_t tmp = (int)(dht.readTemperature() * 10);
    uint8_t hmd = (int)(dht.readHumidity() * 10);
    sprintf(m, "snr:%d~%d|%d~%d|%d~%d|%d", gen.value, city.value, A::fsm.state(), A::error, tmp, hmd, timer);
    Serial.println(m);
    MSG::pub(m);
  }

  void handle(char* inp) {
    char* fn = strtok (inp, "=");
    char* arg = strtok(NULL, "=");

    if (strcmp(fn, "get servo_off") == 0)       { getServo(SERVO_STATES::OFF); } 
    else if (strcmp(fn, "get servo_on") == 0)   { getServo(SERVO_STATES::ON); } 
    else if (strcmp(fn, "scan") == 0)           { scan(); } 
    else if (strcmp(fn, "start") == 0)          { A::trigger(A::EVT::START); }
    else if (strcmp(fn, "stop") == 0)           { A::trigger(A::EVT::STOP); } 
    else if (strcmp(fn, "set servo_on") == 0)   { setServo(atoi(arg), SERVO_STATES::ON); } 
    else if (strcmp(fn, "set servo_off") == 0)  { setServo(atoi(arg), SERVO_STATES::OFF); } 
    else if (strcmp(fn, "reset") == 0)          { rst(); }
  }

  void init() {
    pinMode(ENV::STARTER_PORT, OUTPUT);
    pinMode(ENV::CITY_PORT, INPUT);
    pinMode(ENV::GEN_PORT, INPUT);
    dht.begin();
    servo.attach(ENV::SERVO_PORT);
    if (EEPROM.read(0) == 42) {
      CHOKE_OFF = EEPROM.read(1);
      CHOKE_ON = EEPROM.read(2);
    }
    getServo(SERVO_STATES::OFF);
    A::init();
  };

  void read() {
    while (Serial.available() > 0) {
      char symbol = Serial.read();
      if ( symbol != '\n' && (i < ENV::CMD_MAX_LENGTH - 1) ) {
        message[i++] = symbol;
      } else {
        message[i] = '\0';
        handle(message);
        i = 0;
      }
    }
  };

  void next() {
    read();
    city.next();
    gen.next();
    A::fsm.check_timed_transitions();
  }
}

cmd.cpp

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

# Automatically added based on includes:
Servo
Ethernet
PubSubClient
DHT sensor library

libraries.txt

#pragma once
#include "Fsm.h"
#include "avr.h"
#include "env.h"
#include "cmd.h"
namespace A {
  uint8_t retries = 0;
  uint8_t error = 0;

  void before_idle() {
    CMD::getServo(CMD::SERVO_STATES::OFF); 
    digitalWrite(ENV::STARTER_PORT, LOW);
    digitalWrite(ENV::IGNITION_PORT, HIGH);
  }

  void on_idle() {
    retries = 0;
    error = 0;
    Serial.println("ON IDLE");
  }
 
  void on_start() {
    Serial.println("ON START:" + (String)retries);
    retries++;
    if (retries > ENV::MAX_START_RETRIES) {
      error = ERRORS::TOO_MANY_ATTEMPTS;
      fsm.trigger(EVT::FAIL);
      return;
    }
    if (retries % 2 == 1)  CMD::getServo(CMD::SERVO_STATES::ON); 
    digitalWrite(ENV::STARTER_PORT, HIGH);
  }

  void after_start() {
    CMD::getServo(CMD::SERVO_STATES::OFF); 
    digitalWrite(ENV::STARTER_PORT, LOW);
  }

  void before_warming() {
    CMD::getServo(CMD::SERVO_STATES::OFF);  
    digitalWrite(ENV::STARTER_PORT, LOW);
  }

  void before_run() { digitalWrite(ENV::POWER_PORT, HIGH); }
  void before_cooling() { digitalWrite(ENV::POWER_PORT, LOW); }

  void on_run() {
    Serial.println("ON RUN");
  }
  void before_stop() {  digitalWrite(ENV::IGNITION_PORT, LOW); }
  void on_failure() {
    Serial.println("ON FAILURE ");
    Serial.print(error);
  }

  void unexpected_stop() { error = ERRORS::UNEXPECTED_ENGINE_OFF; }

  State idle( &before_idle, &on_idle, NULL, STATES::IDLE );
  State start( NULL, &on_start, &after_start, STATES::STARTING );
  State warming( &before_warming, NULL, NULL, STATES::WARMING );
  State cooling( &before_cooling, NULL, NULL, STATES::COOLING );
  State recharge( NULL, NULL, NULL, STATES::RECHARGE );
  State run( &before_run, &on_run, NULL, STATES::RUNING );
  State stop( &before_stop, NULL, NULL, STATES::STOPPING );
  State failure( NULL, &on_failure, NULL, STATES::FAILURE );

  Fsm fsm(&idle);

  void trigger( uint8_t evt ) {
    fsm.trigger(evt);
    fsm.run_machine();
  }

  void init() {
    fsm.add_transition(&idle, &start, EVT::START, NULL);
    fsm.add_transition(&start, &warming, EVT::ENGINE_ON, NULL);
    fsm.add_timed_transition(&warming, &run, ENV::WARM_TIME_MS, NULL);
    fsm.add_transition(&warming, &stop, EVT::STOP, NULL);

    fsm.add_transition(&idle, &run, EVT::ENGINE_ON, NULL);
    fsm.add_transition(&recharge, &run, EVT::ENGINE_ON, NULL);
    fsm.add_transition(&start, &failure, EVT::FAIL, NULL);
    fsm.add_timed_transition(&start, &recharge, ENV::STARTING_TIME_MS, NULL);
    fsm.add_timed_transition(&recharge, &start, ENV::RECHARGE_TIME_MS, NULL);
    fsm.add_transition(&run, &failure, EVT::ENGINE_OFF, &unexpected_stop);
    fsm.add_transition(&stop, &idle, EVT::ENGINE_OFF, NULL);
    fsm.add_transition(&run, &cooling, EVT::STOP, NULL);

    fsm.add_timed_transition(&cooling, &stop, ENV::WARM_TIME_MS, NULL);
    fsm.add_transition(&cooling, &run, EVT::START, NULL);
    fsm.run_machine();
  }
}

avr.cpp

#pragma once
#include "Fsm.h"
namespace A {
  void init();
  void trigger( uint8_t state );
  enum ERRORS { TOO_MANY_ATTEMPTS = 91 , UNEXPECTED_ENGINE_OFF = 92};
  enum EVT { START = 21, STOP = 22, LIGHT_ON = 23, LIGHT_OFF = 24, ENGINE_ON = 25, ENGINE_OFF = 26, FAIL = 29};
  enum STATES { IDLE = 1, RECHARGE = 2, STOPPING = 3, COOLING = 4, STARTING = 5, WARMING = 7, RUNING = 8, FAILURE = 0 };
  extern Fsm fsm;
  extern uint8_t retries;
  extern uint8_t error;
}

avr.h

#ifndef FSM_H
#define FSM_H


#if defined(ARDUINO) && ARDUINO >= 100
  #include <Arduino.h>
#else
  #include <WProgram.h>
#endif


struct State {
  State(void (*on_enter)(), void (*on_state)(), void (*on_exit)(), uint8_t id);
  void (*on_enter)();
  void (*on_state)();
  void (*on_exit)();
  uint8_t id;
};


class Fsm {
public:
  Fsm(State* initial_state);
  ~Fsm();

  void add_transition(State* state_from, State* state_to, uint8_t event, void (*on_transition)());

  void add_timed_transition(State* state_from, State* state_to, unsigned long interval, void (*on_transition)());

  void check_timed_transitions();

  void trigger(uint8_t event);
  void run_machine();
  uint8_t state();
  unsigned long triggered;

private:
  struct Transition   {
    State* state_from;
    State* state_to;
    uint8_t event;
    void (*on_transition)();
  };
  struct TimedTransition {
    Transition transition;
    unsigned long start;
    unsigned long interval;
  };

  static Transition create_transition(State* state_from, State* state_to, uint8_t event, void (*on_transition)());

  void make_transition(Transition* transition);

private:
  State* m_current_state;
  Transition* m_transitions;
  int m_num_transitions;

  TimedTransition* m_timed_transitions;
  int m_num_timed_transitions;
  bool m_initialized;
};


#endif

Fsm.h

#include "Fsm.h"
#include <Arduino.h>


State::State(void (*on_enter)(), void (*on_state)(), void (*on_exit)(), uint8_t id) : on_enter(on_enter), on_state(on_state), on_exit(on_exit), id(id) {}

Fsm::Fsm(State* initial_state): m_current_state(initial_state), m_transitions(NULL), m_num_transitions(0), m_num_timed_transitions(0), m_initialized(false) {
  triggered = 0;
}


Fsm::~Fsm() {
  free(m_transitions);
  free(m_timed_transitions);
  m_transitions = NULL;
  m_timed_transitions = NULL;
}


void Fsm::add_transition(State* state_from, State* state_to, uint8_t event, void (*on_transition)()) {
  if (state_from == NULL || state_to == NULL) return;

  Transition transition = Fsm::create_transition(state_from, state_to, event, on_transition);
  m_transitions = (Transition*) realloc(m_transitions, (m_num_transitions + 1) * sizeof(Transition));
  m_transitions[m_num_transitions] = transition;
  m_num_transitions++;
}


void Fsm::add_timed_transition(State* state_from, State* state_to, unsigned long interval, void (*on_transition)()) {
  if (state_from == NULL || state_to == NULL) return;

  Transition transition = Fsm::create_transition(state_from, state_to, 0, on_transition);

  TimedTransition timed_transition;
  timed_transition.transition = transition;
  timed_transition.start = 0;
  timed_transition.interval = interval;

  m_timed_transitions = (TimedTransition*) realloc(
      m_timed_transitions, (m_num_timed_transitions + 1) * sizeof(TimedTransition));
  m_timed_transitions[m_num_timed_transitions] = timed_transition;
  m_num_timed_transitions++;
}


Fsm::Transition Fsm::create_transition(State* state_from, State* state_to, uint8_t event, void (*on_transition)()) {
  Transition t;
  t.state_from = state_from;
  t.state_to = state_to;
  t.event = event;
  t.on_transition = on_transition;

  return t;
}

void Fsm::trigger(uint8_t event) {
  if (m_initialized) {
    // Find the transition with the current state and given event.
    for (int i = 0; i < m_num_transitions; ++i) {
      if (m_transitions[i].state_from == m_current_state && m_transitions[i].event == event) {
        Fsm::make_transition(&(m_transitions[i]));
        return;
      }
    }
  }
}

uint8_t Fsm::state() { return m_current_state->id; }

void Fsm::check_timed_transitions() {
  for (int i = 0; i < m_num_timed_transitions; ++i) {
    TimedTransition* transition = &m_timed_transitions[i];
    if (transition->transition.state_from == m_current_state) {
      if (transition->start == 0) {
        transition->start = millis();
      } else {
        unsigned long now = millis();
        if (now - transition->start >= transition->interval) {
          Fsm::make_transition(&(transition->transition));
          transition->start = 0;
        }
      }
    }
  }
}

void Fsm::run_machine() {
  // first run must exec first state "on_enter"
  if (!m_initialized) {
    m_initialized = true;
    if (m_current_state->on_enter != NULL) m_current_state->on_enter();
  }
  Fsm::check_timed_transitions();
}

void Fsm::make_transition(Transition* transition) {
  // Execute the handlers in the correct order.
  
  if (transition->state_from->on_exit != NULL) transition->state_from->on_exit();
  if (transition->on_transition != NULL) transition->on_transition();
  if (transition->state_to->on_enter != NULL) transition->state_to->on_enter();
  
  m_current_state = transition->state_to;


  if (m_current_state->on_state != NULL) m_current_state->on_state();

  //Initialice all timed transitions from m_current_state
  unsigned long now = millis();
  triggered = now;
  for (int i = 0; i < m_num_timed_transitions; ++i) {
    TimedTransition* ttransition = &m_timed_transitions[i];
    if (ttransition->transition.state_from == m_current_state) ttransition->start = now;
  }
}

Fsm.cpp

#ifndef ENV_H
#define ENV_H

namespace ENV {
  const byte ID = 1;

  const byte STARTER_PORT = 10;
  const byte IGNITION_PORT = 8;
  const byte SERVO_PORT = 9;
  const byte CITY_PORT = 3;
  const byte GEN_PORT = 4;
  const byte POWER_PORT = 7;
  const byte DHT_PIN = 2;
  const byte MAX_START_RETRIES = 5;

  const unsigned long WARM_TIME_MS = 10000;
  const unsigned long STARTING_TIME_MS = 8000;
  const unsigned long RECHARGE_TIME_MS = 10000;
  const unsigned long CITY_DEBOUNCE_MS = 5000;
  const unsigned long GEN_DEBOUNCE_MS = 200;

  const uint8_t CMD_MAX_LENGTH = 30;

  inline constexpr char* TOPIC_CMD = "avr/cmd/1";
  inline constexpr char* TOPIC_EVENT = "avr/event/1";
  inline constexpr char* MQTT_DOMAIN = "broker.emqx.io";
  const unsigned int MQTT_PORT = 1883;

  inline constexpr byte MAC[] = {0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED};
  inline constexpr byte IP_ADDR[]  = { };
  inline constexpr byte IP_NET[] = { 255, 255, 255, 0 };
  inline constexpr byte IP_GW[] = { 192, 168, 177, 1 };
  inline constexpr byte IP_DNS[] = { 192, 168, 177, 1 };

}
#endif

env.h

#pragma once
#include <Arduino.h>
#include <Servo.h>
#include <DHT.h>
#include <Servo.h>
#include <EEPROM.h>
#include "env.h"
#include "msg.h"
#include "avr.h"
namespace CMD {
  enum SERVO_STATES { ON, OFF };
  void init();
  void next();
  void handle(char*);
  void getServo(enum SERVO_STATES state);
  extern Servo servo;
}

cmd.h

#include "msg.h"

namespace MSG {
  EthernetClient ether;
  PubSubClient client(ether);

  void pub(char* payload) {
    client.publish(ENV::TOPIC_EVENT, payload);
  }

  void callback(char* topic, byte* message, unsigned int length) {
    char cmd[length + 1];
    for (uint8_t i = 0; i < length; i++) cmd[i] = (char)message[i];
    cmd[length] = '\0';
    CMD::handle(cmd);
  }

  void begin() {
    Serial.println(F("Begin Ethernet"));

    //Ethernet.init(5);
    
    if (sizeof(ENV::IP_ADDR) > 0 ) {
      Ethernet.begin(ENV::MAC, ENV::IP_ADDR, ENV::IP_DNS, ENV::IP_GW, ENV::IP_NET);
    } else if (Ethernet.begin(ENV::MAC)) {
      Serial.println("DHCP OK!");
      Serial.println(Ethernet.localIP());
    }
    client.setServer(ENV::MQTT_DOMAIN, ENV::MQTT_PORT);
    client.setCallback(callback);
  }

  void reconnect() {
    while (!client.connected()) {
      char clientId[50];
      sprintf(clientId, "clientId-%ld", random(1000));
      if (client.connect(clientId)) client.subscribe(ENV::TOPIC_CMD);
    }
  }
  void loop() {
    if (!client.connected()) reconnect();
    client.loop();
  }
};

msg.cpp

#include <Ethernet.h>
#include <PubSubClient.h>
#include <Arduino.h>
#include "env.h"
#include "cmd.h"

namespace MSG {
  
  void begin();
  void pub(char*);
  void reconnect();
  void loop();
}

msg.h

#include <Arduino.h>
class Sensor {
  private:
    uint8_t port;
    unsigned long debounce;
    unsigned long prev;
    unsigned long now;
    
    uint8_t current;
  public:
    Sensor( unsigned long debounce, uint8_t port, void (*on_change)(uint8_t) ) 
      : debounce(debounce), port(port), on_change(on_change) 
    {
        next();
    }
    uint8_t value;
    void (*on_change)(uint8_t);
    
    bool next() {
      current = digitalRead(port) == HIGH ? 1 : 0;
      now = millis();
      if (current != value) {
        value = current;
        prev = now;
        return false;
      } else if (current == value && now > prev + debounce) {
        on_change(value);
        return true;
      } else {
        return false;
      }
    }
};