Servo Light Switch (mqtt)

/************************************************************************************/

#include <WiFi.h>
#include <PubSubClient.h>
#include <ESP32_Servo.h>

/************ WIFI and MQTT INFORMATION (CHANGE THESE FOR YOUR SETUP) ******************/
#define wifi_ssid "NOTMINE" //enter your WIFI SSID
#define wifi_password "NOTMINE" //enter your WIFI Password

#define mqtt_server "192.168.1.1" // mqtt server
#define mqtt_user "" //enter your MQTT username
#define mqtt_password "" //enter your password
/*************************************************************************************/

/************************************VARIABLE SETUP***********************************/
WiFiClient espClient;
PubSubClient client(espClient);
Servo bottom;
Servo top;
int minUs = 500;
int maxUs = 2400;
int itsatrap = 0;




/************************************* VOID SETUP ****************************************/
void setup() {
  Serial.begin(115200);

  setup_wifi();

  client.setServer(mqtt_server, 1883); //CHANGE PORT HERE IF NEEDED
  client.setCallback(callback);
  pinMode(4, OUTPUT);
  top.attach(15, minUs, maxUs);
  bottom.attach(14, minUs, maxUs);

  //  Serial.println();
  //  Serial.print("MAC: ");
  //  Serial.println(WiFi.macAddress());



}
/*************************************************************************************/

/******************************** VOID WIFI SETUP *************************************/
void setup_wifi() {

  delay(10);
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(wifi_ssid);

  WiFi.mode(WIFI_STA);
  WiFi.begin(wifi_ssid, wifi_password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
}
/*************************************************************************************/

/*********************************** IMPORTANT LOOP **********************************/

void callback(char* topic, byte* payload, unsigned int length) {
  char p[length + 1];
  memcpy(p, payload, length);
  p[length] = NULL;
  String message(p);
  String mytopic(topic);
  Serial.println(mytopic);

  if (itsatrap == 0 && mytopic == "left/command" && message.equals("ON")) {
    
    bottom.write(0);
    client.publish("left/state", "ON");
    delay(500);
    bottom.write(90);

  }
  else if (mytopic == "left/command" && message.equalsIgnoreCase("OFF")) {

    top.write(180);
    client.publish("left/state", "OFF");
    delay(500);
    top.write(90);


  }
  else {
    itsatrap = 0;
  }

  if (itsatrap == 0 && mytopic == "right/direct" && message.equalsIgnoreCase("ON")) {

    bottom.write(180);
    client.publish("right/state", "ON");
    delay(500);
    bottom.write(90);

  }
  else if (mytopic == "right/direct" && message.equalsIgnoreCase("OFF")) {


    top.write(0);
    client.publish("right/state", "OFF");
    delay(500);
    top.write(90);

  }
  else {
    itsatrap = 0;
  }

  if (itsatrap == 0 && mytopic == "light/command" && message.equalsIgnoreCase("OFF")) {
    digitalWrite(4, LOW); //LED on
  }
  else if (mytopic == "light/command" && message.equalsIgnoreCase("ON")) {
    digitalWrite(4, HIGH); //LED off
  }

   else {
    itsatrap = 0;
  }

}
/*************************************************************************************/

/******************************** VOID LOOP ******************************************/
void loop() {

  if (!client.connected()) {
    reconnect();
  }



  client.loop();
}

/*************************************************************************************/

/***********************************VOID RECONNECT ***********************************/

void reconnect() {
  // Loop until we're reconnected
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    // Attempt to connect
    if (client.connect("ESPSwitch", mqtt_user, mqtt_password)) {
      Serial.println("connected");


      client.subscribe("left/command");
      client.subscribe("right/direct");
      client.subscribe("light/command");



    } else {
      Serial.print("failed, rc=");
      // Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}
/***************************************END END END END ********************************/