#include <ESP32Servo.h>
Servo myservo;

#define DCmotor1 22
#define DCmotor2 19
#define DCmotor3 5
#define SwitchDCmotor 23
#define servoPin 32
#define Switchservo 35
int pos = 0;
int count = 0;
int countold = 0;
bool dir = true;
bool prInt = true;

void IRAM_ATTR openDCmotor()
{
  countold = count;
  count += 1;
  if(count == 4){
    count = 0;
  }
  prInt = true;
}

void setup() {
  pinMode(DCmotor1 , OUTPUT);
  pinMode(DCmotor2 , OUTPUT);
  pinMode(DCmotor3 , OUTPUT);
  pinMode(SwitchDCmotor , INPUT);
  pinMode(Switchservo , INPUT);
  myservo.attach(servoPin);
  attachInterrupt(SwitchDCmotor, openDCmotor, RISING);
  Serial.begin(9600);
}

void loop() {
  if(digitalRead(Switchservo) == HIGH){
    if (dir){
      if (pos>0){
        myservo.write(pos); // Set the arm's position to "pos" value
        pos-= 1; // Decrement "pos" of "step" value
        delay(15); // Wait 5ms for the arm to reach the position
      }else{
        dir = !dir;
      }
    }
    if (!dir){
      if (pos<180){
        myservo.write(pos); // Set the arm's position to "pos" value
        pos+= 1; // Increment "pos" of "step" value
        delay(15); // Wait 5ms for the arm to reach the position
      }else{
        dir = !dir;
      }
    }
  }
  if(prInt){
    //Serial.println(countold + "-->" + count);
    Serial.println(count);
    prInt = false;
  }
  if(count == 1){
    digitalWrite(DCmotor1, HIGH);
    digitalWrite(DCmotor2, LOW);
    digitalWrite(DCmotor3, LOW);
  }else if(count == 2){
    digitalWrite(DCmotor1, LOW);
    digitalWrite(DCmotor2, HIGH);
    digitalWrite(DCmotor3, LOW);
  }else if(count == 3){
    digitalWrite(DCmotor1, LOW);
    digitalWrite(DCmotor2, LOW);
    digitalWrite(DCmotor3, HIGH);
  }else{
    digitalWrite(DCmotor1, LOW);
    digitalWrite(DCmotor2, LOW);
    digitalWrite(DCmotor3, LOW);
  }
}