Cod Mega

#include <LiquidCrystal_I2C.h> // Library for LCD
#include <dht.h>
#include "ServoTimer2.h"
#include <SoftwareSerial.h>
#include <ArduinoJson.h>
SoftwareSerial s(22,24);
LiquidCrystal_I2C lcd = LiquidCrystal_I2C(0x27, 16, 2);
dht DHT;
ServoTimer2 Servo1;
const int stepPin = 12;
const int dirPin = 13;
const int stepPin2 = 10;
const int dirPin2 = 11;
const int stepPin3 = 6;
const int dirPin3 = 5;
const int RelayPin =30;
const int RelayPin2 = 28;
const int L1 = 50;
const int L2 = 48;
const int L3 = 46;
const int L4 = 44;
const int L5 = 42;
const int L6 = 40;
const int inX = A3;
const int inY = A2;
const int inX2 = A4;
const int inY2 = A5;
const int inX3 = A6;
const int inY3 = A7;
const int inX4 = A8;
const int inY4 = A9;
const int inX5 = A10;
const int inY5 = A11;
int servoPin = 2;
int xValue = 0;
int yValue = 0;
int xValue2 = 0;
int yValue2 = 0;
int xValue3 = 0;
int yValue3 = 0;
int xValue4 = 0;
int yValue4 = 0;
int xValue5 = 0;
int yValue5 = 0;
int tmp=0;
int hum=0;
int CurrentState=0;
bool pornit = 0;
const int StartP = 8;
const int StopP = 9;
const int Mod = 38;
int ht=0;
bool Md = 0;
const int PIR1 =34 ;
bool pir1=0;
const int PIR2 = 32;
bool pir2=0;
const int senzor_nivel= A12;
float delayMinMotor = 500 * 0.062;
float delayMinMotor2 = 300 * 0.062; //
void setup() {
  lcd.init();
  lcd.backlight();
  pinMode(StartP, INPUT);
  pinMode(StopP, INPUT);
  pinMode(L1, INPUT);
  pinMode(Mod, INPUT_PULLUP);
  pinMode(inX, INPUT);
  pinMode(inY, INPUT);
  pinMode(inX2, INPUT);
  pinMode(inY2, INPUT);
  pinMode(inX3, INPUT);
  pinMode(inY3, INPUT);
  pinMode(inX4, INPUT);
  pinMode(inY4, INPUT);
  pinMode(inX5, INPUT);
  pinMode(inY5, INPUT);
  pinMode(PIR1, INPUT);
  pinMode(PIR2, INPUT);
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);
  pinMode(stepPin3, OUTPUT);
  pinMode(dirPin3, OUTPUT);
  pinMode(RelayPin, OUTPUT);
  pinMode(RelayPin2, OUTPUT);
  digitalWrite(RelayPin, HIGH);
  digitalWrite(RelayPin2, HIGH);
  Servo1.attach(2);
  Servo1.write(1000);
  //Serial.begin(9600);
  s.begin(9600);
  cli();
  TCCR4A = 0;
  TCCR4B = 0;
  TCNT4 = 0;
  OCR4A = delayMinMotor2  ;
  TCCR4B |= (1 << WGM42);
  TCCR4B |= (1 << CS42);
  TCCR3A = 0;
  TCCR3B = 0;
  TCNT3 = 0;
  OCR3A = delayMinMotor;
  TCCR3B |= (1 << WGM32);
  TCCR3B |= (1 << CS32);
  sei();
  stopTimer3();
  stopTimer4();
}
void loop()
{
  Servo1.detach();

  TIMSK4 |= (1 << OCIE1A);
  TIMSK3 |= (1 << OCIE3A);
  Md = digitalRead(Mod);
  ht= DHT.read11(A13);
  tmp = DHT.temperature;
  hum = DHT.humidity;

   StaticJsonDocument<1500> doc;
   JsonObject root = doc.to<JsonObject>();
   root["pornit"] = pornit;
   root["mod"] = Md;
   root["lim1"] = digitalRead(L1);
   root["lim2"] = digitalRead(L2);
   root["lim3"] = digitalRead(L3);
   root["lim4"] = digitalRead(L4);
   root["lim5"] = digitalRead(L5);
   root["lim6"] = digitalRead(L6);
   root["pir1"] = digitalRead(PIR1);
   root["pir2"] = digitalRead(PIR2);
   root["apa"] = analogRead(senzor_nivel);
   root["temperatura"] = tmp;
   serializeJson(doc,s);
  if( Md == 1)
{
    lcd.clear();
    lcd.setCursor(5, 0);
    lcd.print("MANUAL");
    read_joystick(inX,inY);
    read_joystick2(inX2,inY2);
    read_joystick3(inX3);
    read_joystick4(inX4);
    read_joystick5(inX5);
    delay(50);
}
else if ( Md == 0)
{
    lcd.clear();
    lcd.setCursor(5, 0);
    lcd.print("AUTOMAT");
if(digitalRead(StartP)== LOW)
    {
      pornit = 1;
   //   stopTimer3();
    // stopTimer4();
    }
if(digitalRead(StopP)== LOW)
    {
      pornit=0;
    }
if(pornit == 1)
    {

     lcd.clear();
     lcd.setCursor(5, 0);
     lcd.print("AUTOMAT");
     lcd.setCursor(0, 1);
     lcd.print("PROCESS STARTED");
     switch(CurrentState){
      case 0:
          {
            deplasare(digitalRead(L1));
            break;
          }
      case 1:
          {
            verificare_pir_2();
            break;
          }
      case 2:
          {
            porneste_banda();
            break;
          }
      case 3:
          {
            verificare_pir_1();
            break;
          }
      case 4:
          {
            opreste_banda();
            break;
          }
      case 5:
          {
            deschide_cleste();
            coboara_brat(10);
            break;
          }
      case 6:
          {
            inchide_cleste();
            urca_brat(8);
            break;
          }
      case 7:
          {
            deplasare(digitalRead(L2));
            break;
          }
      case 8:
          {
            verificare_senzor_nivel(senzor_nivel);
            break;
          }
      case 9:
          {
            coboara_brat(13);
            break;
          }
      case 10:
          {
            urca_brat(11);
            break;
          }
      case 11:
          {
            deplasare(digitalRead(L3));
            break;
           }

   case 12 :
          {
            coboara_brat(13);
            break;
          }
    case 13:
          {
            urca_brat(11);
            break;
          }
    case 14:
          {
            deplasare(digitalRead(L5));
            break;
          }
    case 15:
          {
            coboara_brat(13);
            break;
          }
    case 16:
          {
            porneste_ventilatoare(30);
            break;
          }
    case 17:
          {
            urca_brat(11);
            break;
          }
    case 18:
          {
            deplasare(digitalRead(L6));
            break;
          }
    case 19:
          {
            coboara_brat(25);
            break;
          }
    case 20:
          {
            deschide_cleste();
            CurrentState++;
            break;
          }
    case 21:
          {
           urca_brat(26);
           break;
          }
    case 22:
          {
            deplasare2(digitalRead(L1));
            break;
          }
        }
  }
else if(pornit==0)
  {
     lcd.clear();
     lcd.setCursor(5, 0);
     lcd.print("AUTOMAT");
     lcd.setCursor(0, 1);
     lcd.print("PROCESS STOPPED");
    stopTimer3();
   stopTimer4();
    }

}
  delay(150);
}

//Functii intrerupere
ISR(TIMER4_COMPA_vect)          // timer 4 compare interrupt service routine misk4
{
   digitalWrite(stepPin,HIGH);
   digitalWrite(stepPin,LOW);
   digitalWrite(stepPin2,HIGH);
   digitalWrite(stepPin2,LOW);
}
ISR(TIMER3_COMPA_vect)          // timer 3 compare interrupt service routine misk3
{
  digitalWrite(stepPin3,HIGH);
  digitalWrite(stepPin3,LOW);;
}
void stopTimer4()
{

  TCCR4B &= ~(1 << CS40);
  TCCR4B &= ~(1 << CS41);
  TCCR4B &= ~(1 << CS42);
  TCNT4 = 0;
}
void startTimer4()
{
  TCCR4B |= (1 << CS42);
}
void stopTimer3()
{

  TCCR3B &= ~(1 << CS30);
  TCCR3B &= ~(1 << CS31);
  TCCR3B &= ~(1 << CS32);
  TCNT3 = 0;
}
void startTimer3()
{
  TCCR3B |= (1 << CS32);
}
//Functii_manual
void read_joystick(int pin1, int pin2)
{
    xValue = analogRead(pin1);
    yValue = analogRead(pin2);
    if (xValue == 0)
    {
        digitalWrite(dirPin,HIGH);
        digitalWrite(dirPin2,HIGH);
        startTimer4();
    }
    if (xValue > 0 && xValue < 1023 )
    {
        stopTimer4();
    }
    if ( xValue == 1023)
    {
        digitalWrite(dirPin,LOW);
        digitalWrite(dirPin2,LOW);
        startTimer4();
    }
}
void read_joystick2(int pin1, int pin2)
{
    xValue2 = analogRead(pin1);
    yValue2 = analogRead(pin2);
    if (xValue2 == 0)
    {
        digitalWrite(dirPin3,HIGH);

        startTimer3();
    }
    if (xValue2 > 0 && xValue2 < 1023 )
    {
        stopTimer3();
    }
    if ( xValue2 == 1023)
    {
        digitalWrite(dirPin3,LOW);

        startTimer3();
    }
}
void read_joystick3(int pin1)
{
    xValue3 = analogRead(pin1);
    if (xValue3 == 1023)
       {
          digitalWrite(RelayPin, LOW);
       }
    if (xValue3  == 0)
       {
          digitalWrite(RelayPin, HIGH);
       }
}
void read_joystick4(int pin1)
{
    xValue4 = analogRead(pin1);
    if (xValue4 == 1023)
       {
          digitalWrite(RelayPin2, LOW);
       }
    if (xValue4  == 0)
       {
          digitalWrite(RelayPin2, HIGH);
       }
}
void read_joystick5(int pin1)
{
    xValue5 = analogRead(pin1);
    if (xValue5  == 1023)
       {
        Servo1.attach(2);
          Servo1.write(1750);


       }
    if (xValue5  == 0)
       {
        Servo1.attach(2);
          Servo1.write(1000);

       }
}
//Functii automat
void deplasare( int lim)
{
  if(lim == 0)
  {
  digitalWrite(dirPin,LOW);
  digitalWrite(dirPin2,LOW);
  startTimer4();
  }
  else
  {
    stopTimer4();
    CurrentState++;
  }
}
void deplasare2( int lim)
{
  if(lim == 0)
  {
  digitalWrite(dirPin,HIGH);
  digitalWrite(dirPin2,HIGH);
  startTimer4();
  }
  else
  {
    stopTimer4();
    CurrentState=0;
  }
}
void verificare_pir_2()
{
  pir2 = digitalRead(PIR2);
  if(pir2 == 0)
  {
    CurrentState++;
  }
}
void porneste_banda()
{
  digitalWrite(RelayPin, LOW);
  CurrentState++;
}
void verificare_pir_1()
{
  pir1 = digitalRead(PIR1);
  if(pir1 == 0)
  {
    CurrentState++;
  }
}
void opreste_banda()
{
  digitalWrite(RelayPin, HIGH);
  CurrentState++;
}
void deschide_cleste()
{
  Servo1.attach(2);
  Servo1.write(1000);

}
 int cnt =0;
void coboara_brat( int durata)
{

  if(cnt<durata)
        {
        digitalWrite(dirPin3,HIGH);
        startTimer3();
        cnt++;
        }
  else
        {
         stopTimer3();
         CurrentState++;
         cnt =0;
        }
}
void inchide_cleste()
{
  Servo1.attach(2);
  Servo1.write(1750);

}
int cnt2 =0;
void urca_brat( int durata)
{

  if(cnt2<durata)
        {
        digitalWrite(dirPin3,LOW);
        startTimer3();
        cnt2++;
        }
  else
        {
         stopTimer3();
         CurrentState++;
         cnt2 =0;
        }
}
int cnt3;
void porneste_ventilatoare(int durata)
{
  if(cnt3<durata)
              {
                   digitalWrite(RelayPin2,LOW);
                   cnt3++;
               }
             else
              {
                    digitalWrite(RelayPin2,HIGH);
                    CurrentState++;
                    cnt3= 0;
              }

}
void verificare_senzor_nivel(int level)
  {
    bool apaok;
    if (level<=480){
    apaok = 0;
  }
  else if (level>480 && level<=530){

  }
  else if (level>530 && level<=615){
    apaok=1;
  }
  else if (level>615 && level<=660){
    apaok=1;
  }
  else if (level>660 && level<=680){
    apaok=1;
  }
  else if (level>680 && level<=690){
    apaok=1;
  }
  else if (level>690 && level<=700){
  }
  else if (level>700 && level<=705){
    apaok=1;
  }
  else if (level>705){
    apaok=1;
  }
  if (apaok == 0)
   {
         CurrentState++;
    }

 }