#pragma once
#include "msg.h"
#include "cmd.h"

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  CMD::init();
  MSG::begin();
}


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


sketch.ino

diagram.json

#pragma once
#include "msg.h"

namespace MSG {
    WiFiClient espClient;
    PubSubClient client(espClient);
    unsigned long prev = millis();
    char clientId[50];

    void pub(char* payload) {
        //char m[30];
        //snprintf(m, sizeof(m), "%d|%d|%d|%.2f", 1, 2, 3, -12.2);
        char t[14];
        snprintf(t, sizeof(t), ENV::TOPIC_SENSOR, ENV::ID);
        client.publish(t, payload);
    }

    void callback(char* topic, byte* message, unsigned int length) {
      //char cmd[length + 1];
      //for (int i = 0; i < length; i++) cmd[i] = (char)message[i];
      //cmd[length] = '\0';
      String cmd;
      for (int i = 0; i < length; i++) cmd += (char)message[i];
      CMD::handle(cmd);

      //Serial.println(cmd);
      //char m[50];
      //if(strcmp(cmd, "scan") == 0) snprintf(m, sizeof(m), "%d|%d|%d|%.2f", 1, 2, 3, -12.2);

      //Serial.println(m);

      //pub(m);
    }

    void begin() {
      WiFi.begin("Wokwi-GUEST", "", 6);
      while (WiFi.status() != WL_CONNECTED) delay(500);
      Serial.println("WiFi connected");
  
      client.setServer("broker.emqx.io", 1883);
      client.setCallback(callback);
    }

    void reconnect() {
      char topic[10];
      snprintf(topic, sizeof(topic), ENV::TOPIC_CMD, ENV::ID);
      while (!client.connected()) {
        long r = random(1000);
        sprintf(clientId, "clientId-%ld", r);
        if (client.connect(clientId)) {
          client.subscribe(topic);
        } else {
          Serial.print("failed, rc=");
          Serial.print(client.state());
          Serial.println(" try again in 5 seconds");
          delay(5000);
        }
      }
    }
    void loop() {
      if (!client.connected()) reconnect();
      client.loop();
    }
};

msg.cpp

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

libraries.txt

#pragma once
#include <WiFi.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

#ifndef ENV_H
#define ENV_H

namespace ENV {
  const byte ID = 1;

  const byte STARTER_PORT = 10;
  const byte IGNITION_PORT = 9;
  const byte SERVO_PORT = 18;
  const byte CITY_PORT = 3;
  const unsigned long CITY_DEBOUNCE_MS = 5000;
  const byte GEN_PORT = 2;
  const unsigned long GEN_DEBOUNCE_MS = 200;
  const unsigned long SENSOR_PUB_INTERVAL_MS = 5000;

  inline constexpr char* TOPIC_CMD = "avr/cmd/%d";
  inline constexpr char* TOPIC_SENSOR = "avr/sensor/%d";
  inline constexpr char* TOPIC_ERROR = "avr/error/%d";

  extern byte CHOKE_OFF;
  extern byte CHOKE_ON;

  void init();
  void save(byte val, char* state);



}
#endif

env.h

#pragma once
#include "cmd.h"
#include "env.h"
#include "msg.h"
namespace CMD {
  Servo servo;
  class Cmd {
    public:
      Cmd() {};
      String fn;
      String arg;
  };


  void setServo(byte pos, char* state) {
    servo.write(pos); 
    ENV::save(pos, state);
  }
  void getServo(char* state) {
    byte pos = state == "off" ? ENV::CHOKE_OFF : ENV::CHOKE_ON;
    servo.write(pos); 
  }
  void scan() {
    char m[30];
    snprintf(m, sizeof(m), "%d|%d|%d|%.2f", 1, 2, 3, -12.2);
    MSG::pub(m);
  }


  Cmd read(String& inp) {
    Cmd cmd;
    
    inp.trim();
    int delim = inp.indexOf("=");
    cmd.fn = inp.substring(0, delim);
    cmd.fn.trim();
    cmd.arg = inp.substring(delim + 1);
    cmd.arg.trim();
    return cmd; 
  }
  void handle(String& inp) {
      Cmd cmd = read(inp);
    
      if (cmd.fn == "set servo_on") setServo(cmd.arg.toInt(), "on");
      else if (cmd.fn == "set servo_off") setServo(cmd.arg.toInt(), "off");
      else if (cmd.fn == "get servo_off") getServo("off");
      else if (cmd.fn == "get servo_on") getServo("on");
      else if (cmd.fn == "scan") scan();
      //else if (cmd.fn == "start") A::trigger(A::STATES::START);
      //else if (cmd.fn == "stop") A::trigger(A::STATES::STOP);
    
  }
  void init() {
    pinMode(ENV::STARTER_PORT, OUTPUT);
    servo.attach(ENV::SERVO_PORT);
  };
  void next() {
    if (Serial.available()) {
      String inp = Serial.readString();
      handle(inp);
    }  
  }
}

cmd.cpp

#pragma once
#include <Arduino.h>
//#include <Servo.h>
#include <ESP32Servo.h>
//#include "avr.h"
namespace CMD {
  extern Servo servo;
  void init();
  void next();
  void handle(String&);
}

cmd.h

#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.write(0, 42);
    if (state == "off") {
      EEPROM.write(1, val);
      CHOKE_OFF = val;
    } else if (state == "on") {
      EEPROM.write(2, val);
      CHOKE_ON = val;
    }
  }
};

