#include <ESP32Servo.h>

Servo servo1;
Servo servo2;

int regim = 0;
int sermot = 0;
int t = 0;
int e = 0;

long count1 = 0;
long count2 = 0;
const int ServoPin1 = 41;
const int ServoPin2 = 40;
int pos = 0;


int pos1_1 = 90;
int pos1_2 = 90;
int pos2_1 = 800;
int pos2_2 = 800;

int pos1_12 = 90;
int pos1_22 = 90;
int pos2_12 = 800;
int pos2_22 = 800;

int endd = 0;

void count_function() {
  if (digitalRead(5)) {count1 ++; count2 ++;}
  else {count1 --; count2 --;}
  //Serial.println(count);
}

void motor1(int shag1, int shag2, int mig) {
  t = 0;
  if (shag2 > shag1) {
    digitalWrite(16, 1);
    for (pos = shag1; pos <= shag2; pos ++) {
      digitalWrite(17, 1);
      delay(15);
      t += 5;
      if (t % 200 < 100 & mig == 1) {
        digitalWrite(46, 1);
        digitalWrite(10, 0);
      }
      else if (mig == 1) {
        digitalWrite(46, 0);
        digitalWrite(10, 1);
      }
      digitalWrite(17, 0);
    }
  }
  else {
    digitalWrite(16, 0);
    for (pos = shag2; pos <= shag1; pos ++) {
      digitalWrite(17, 1);
      delay(5);
      t += 5;
      if (t % 200 < 100 & mig == 1) {
        digitalWrite(46, 1);
        digitalWrite(10, 0);
      }
      else if (mig == 1) {
        digitalWrite(46, 0);
        digitalWrite(10, 1);
      }
      digitalWrite(17, 0);
    }
  }
}

void motor2(int shag1, int shag2, int mig) {
  t = 0;
  if (shag2 > shag1) {
    digitalWrite(18, 1);
    for (pos = shag1; pos <= shag2; pos ++) {
      digitalWrite(8, 1);
      delay(5);
      t += 5;
      if (t % 200 < 100 & mig == 1) {
        digitalWrite(46, 1);
        digitalWrite(10, 0);
      }
      else if (mig == 1) {
        digitalWrite(46, 0);
        digitalWrite(10, 1);
      }
      digitalWrite(8, 0);
    }
  }
  else {
    digitalWrite(18, 0);
    for (pos = shag2; pos <= shag1; pos ++) {
      digitalWrite(8, 1);
      delay(5);
      t += 5;
      if (t % 200 < 100 & mig == 1) {
        digitalWrite(46, 1);
        digitalWrite(10, 0);
      }
      else if (mig == 1) {
        digitalWrite(46, 0);
        digitalWrite(10, 1);
      }
      digitalWrite(8, 0);
    }
  }
}

void motor12(int shag1, int shag2, int mig) {
  t = 0;
  if (shag2 > shag1) {
    digitalWrite(16, 1);
    digitalWrite(18, 1);
    for (pos = shag1; pos <= shag2; pos ++) {
      digitalWrite(17, 1);
      digitalWrite(8, 1);
      delay(5);
      t += 5;
      if (t % 200 < 100 & mig == 1) {
        digitalWrite(46, 1);
        digitalWrite(10, 0);
      }
      else if (mig == 1) {
        digitalWrite(46, 0);
        digitalWrite(10, 1);
      }
      digitalWrite(17, 0);
      digitalWrite(8, 0);
    }
  }
  else {
    digitalWrite(16, 0);
    digitalWrite(18, 0);
    for (pos = shag2; pos <= shag1; pos ++) {
      digitalWrite(17, 1);
      digitalWrite(8, 1);
      delay(5);
      t += 5;
      if (t % 200 < 100 & mig == 1) {
        digitalWrite(46, 1);
        digitalWrite(10, 0);
      }
      else if (mig == 1) {
        digitalWrite(46, 0);
        digitalWrite(10, 1);
      }
      digitalWrite(17, 0);
      digitalWrite(8, 0);
    }
  }
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  attachInterrupt(4, count_function, FALLING);
  //Serial.println("Hello, ESP32-S3!");

  pinMode(5, INPUT);
  pinMode(4, INPUT);

  pinMode(20, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(46, OUTPUT);
  pinMode(10, OUTPUT);

  pinMode(40, OUTPUT);
  pinMode(35, OUTPUT);
  pinMode(37, OUTPUT);

  pinMode(17, OUTPUT);
  pinMode(16, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(18, OUTPUT);

  pinMode(37, INPUT);
  pinMode(35, INPUT);
  pinMode(39, INPUT);
  //for (int i = 1; i <= 5; i ++) {
  //  digitalWrite(20, 0);
  //  digitalWrite(3, 1);
  //  digitalWrite(46, 1);
  //  digitalWrite(10, 0);
  //  delay(500);
  //  digitalWrite(20, 1);
  //  digitalWrite(3, 0);
  //  digitalWrite(46, 0);
  //  digitalWrite(10, 1);
  //  delay(500);
  //}

  servo1.attach(ServoPin1, 500, 2400);
  servo2.attach(ServoPin2, 500, 2400);

  servo1.write(90);
  servo2.write(90);
  delay(500);
  digitalWrite(16, 1);
  digitalWrite(18, 1);
  int led = 0;

  for (pos = 800; pos <= 1600; pos += 1) {
    servo1.write(map(pos, 800, 1600, 90, 180));
    servo2.write(map(pos, 800, 1600, 90, 180));
     digitalWrite(17, 1);
    digitalWrite(8, 1);
    delay(5);
    led += 5;
    digitalWrite(17, 0);
    digitalWrite(8, 0);
    if ((led % 1000) <= 500) {
      digitalWrite(20, 0);
      digitalWrite(3, 1);
      digitalWrite(46, 1);
      digitalWrite(10, 0);
    }
    else {
      digitalWrite(20, 1);
      digitalWrite(3, 0);
      digitalWrite(46, 0);
      digitalWrite(10, 1);
    }
    //Serial.println(led % 1000);
  }
  digitalWrite(16, 0);
  digitalWrite(18, 0);
  led = 0;
  for (pos = 1600; pos >= 0; pos -= 1) {
    servo1.write(map(pos, 0, 1600, 0, 180));
    servo2.write(map(pos, 0, 1600, 0, 180));
    digitalWrite(17, 1);
    digitalWrite(8, 1);
    delay(5);
    led += 5;
    if (led <= 500) {
      digitalWrite(20, 0);
      digitalWrite(3, 1);
      digitalWrite(46, 1);
      digitalWrite(10, 0);
    }
    else {
      digitalWrite(20, 1);
      digitalWrite(3, 0);
      digitalWrite(46, 0);
      digitalWrite(10, 1);
    }
    digitalWrite(17, 0);
    digitalWrite(8, 0);
  }
  digitalWrite(16, 1);
  digitalWrite(18, 1);
  for (pos = 0; pos <= 800; pos += 1) {
    servo1.write(map(pos, 0, 800, 0, 90));
    servo2.write(map(pos, 0, 800, 0, 90));
    digitalWrite(17, 1);
    digitalWrite(8, 1);
    delay(5);
    digitalWrite(17, 0);
    digitalWrite(8, 0);
    
  }


  //if (!digitalRead(39)) {
  //  regim += 1;
  //  sermot = 1;
  //  if (regim > 4) {regim = 1;}
  //  while (!digitalRead(39)) {delay(10);}
  //}
  sermot = 1;
  regim = 1;
}


void loop() {
  // put your main code here, to run repeatedly:
  //delay(10); // this speeds up the simulation
  //int a = digitalRead(39);
  //Serial.println(a);
  //delay(100);
  //if (digitalRead(37)) {
  //  regim += 1;
  //  sermot = 2;
  //  while (digitalRead(37)) {delay(1);}
  //}


  if (sermot == 1) {
    if (regim == 1) {
      digitalWrite(20, 0);
      digitalWrite(3, 1);
      digitalWrite(46, 0);
      digitalWrite(10, 1);
      endd = 0;
      count1 = 0;
      count2 = 0;
      Serial.println(1);
      while (endd == 0) {
        pos1_12 = pos1_1 + count1 * 8;
        pos1_22 = pos1_2 + count2 * 8;
        if (0 > pos1_12 || pos1_12 > 180) {count1 = 0; pos1_1 = pos1_12;}
        if (0 > pos1_22 || pos1_22 > 180) {count2 = 0; pos1_2 = pos1_22;}
        pos1_12 = constrain(pos1_12, 0, 180);
        pos1_22 = constrain(pos1_22, 0, 180);
        pos1_1 = constrain(pos1_1, 0, 180);
        pos1_2 = constrain(pos1_2, 0, 180);
        servo1.write(pos1_12);
        servo2.write(pos1_22);

        if (digitalRead(39)) {
          endd = 1;
          while (digitalRead(39)) {delay(10);}
          Serial.println(1);
        }
        if (digitalRead(37)) {
          endd = 2;
          while (digitalRead(37)) {delay(10);}
          Serial.println(2);
        }
        if (digitalRead(35)) {
          endd = 3;
          while (digitalRead(35)) {delay(10);}
          Serial.println(3);
        }
      }
      pos1_1 = constrain(pos1_12, 0, 180);
      pos1_2 = constrain(pos1_22, 0, 180);
    }
    if (regim == 2) {
      digitalWrite(20, 0);
      digitalWrite(3, 0);
      digitalWrite(46, 0);
      digitalWrite(10, 1);
      endd = 0;
      count1 = 0;
      count2 = 0;
      Serial.println(2);
      while (endd == 0) {
        pos1_12 = pos1_1 + count1 * 8;
        if (0 > pos1_12 || pos1_12 > 180) {count1 = 0; pos1_1 = pos1_12;}
        pos1_12 = constrain(pos1_12, 0, 180);
        pos1_1 = constrain(pos1_1, 0, 180);
        servo1.write(pos1_12);
        if (digitalRead(39)) {
          endd = 1;
          while (digitalRead(39)) {delay(10);}
          Serial.println(2);
        }
        if (digitalRead(37)) {
          endd = 2;
          while (digitalRead(37)) {delay(10);}
          Serial.println(2);
        }
        if (digitalRead(35)) {
          endd = 3;
          while (digitalRead(35)) {delay(10);}
          Serial.println(3);
        }
      }
      pos1_1 = constrain(pos1_12, 0, 180);
    }
    if (regim == 3) {
      digitalWrite(20, 1);
      digitalWrite(3, 1);
      digitalWrite(46, 0);
      digitalWrite(10, 1);
      endd = 0;
      count1 = 0;
      count2 = 0;
      Serial.println(3);
      while (endd == 0) {
        pos1_22 = pos1_2 + count2 * 8;
        if (0 > pos1_22 || pos1_22 > 180) {count2 = 0; pos1_2 = pos1_22;}
        pos1_22 = constrain(pos1_22, 0, 180);
        pos1_2 = constrain(pos1_2, 0, 180);
        servo2.write(pos1_22);
        if (digitalRead(39)) {
          endd = 1;
          while (digitalRead(39)) {delay(10);}
          Serial.println(3);
        }
        if (digitalRead(37)) {
          endd = 2;
          while (digitalRead(37)) {delay(10);}
          Serial.println(2);
        }
        if (digitalRead(35)) {
          endd = 3;
          while (digitalRead(35)) {delay(10);}
          Serial.println(3);
        }
      }
      pos1_2 = constrain(pos1_22, 0, 180);
    }
    if (regim == 4) {
      digitalWrite(46, 0);
      digitalWrite(10, 1);
      t = 0;
      endd = 0;
      count1 = 0;
      count2 = 0;
      Serial.println(4);
      while (endd == 0) {
        if (t % 200 < 100) {
          digitalWrite(20, 0);
          digitalWrite(3, 1);
        }
        else {
          digitalWrite(20, 1);
          digitalWrite(3, 0);
        }
        pos1_22 = pos1_2 + count2 * 8;
        if (0 > pos1_22 || pos1_22 > 180) {count2 = 0; pos1_2 = pos1_22;}
        pos1_22 = constrain(pos1_22, 0, 180);
        pos1_2 = constrain(pos1_2, 0, 180);
        servo1.write(180 - pos1_22);
        servo2.write(pos1_22);
        if (digitalRead(39)) {
          endd = 1;
          while (digitalRead(39)) {delay(10);}
          Serial.println(4);
        }
        if (digitalRead(37)) {
          endd = 2;
          while (digitalRead(37)) {delay(10);}
          Serial.println(2);
        }
        if (digitalRead(35)) {
          endd = 3;
          while (digitalRead(35)) {delay(10);}
          Serial.println(3);
        }
        delay(1);
        t ++;
      }
      pos1_2 = constrain(pos1_22, 0, 180);
      pos1_1 = 180 - constrain(pos1_22, 0, 180);
    }
    if (endd == 1) {
      regim += 1; 
      if (regim > 4) {regim = 1;}; 
      endd = 0;
    }
    if (endd == 2) {sermot = 2; regim = 1; endd = 0;}
    //if (endd == 3) {sermot = 3; regim = 1; endd = 0;}
  }


  if (sermot == 2) {
    if (regim == 1) {                      //regim1
      Serial.println(123);
      digitalWrite(20, 1);
      digitalWrite(3, 0);
      digitalWrite(46, 1);
      digitalWrite(10, 0);
      endd = 0;
      count1 = 0;
      count2 = 0;
      while (endd == 0) {
        t = 0;
        while (t <= 1000) {
          if (digitalRead(39)) {
            endd = 1;
            while (digitalRead(39)) {delay(10);}
            Serial.println(4);
          }
          if (digitalRead(37)) {
            endd = 2;
            while (digitalRead(37)) {delay(10);}
            Serial.println(2);
          }
          if (digitalRead(35)) {
            endd = 3;
            while (digitalRead(35)) {delay(10);}
            Serial.println(3);
          }
          delay(1);
          t += 1;
        }
        pos2_12 = constrain(pos2_1 + count1 * 64, 0, 1600);
        pos2_22 = constrain(pos2_2 + count2 * 64, 0, 1600);

        if (pos2_12 > pos2_1) {
          digitalWrite(16, 1);
          digitalWrite(18, 1);
          Serial.print(pos2_1);
          Serial.print("  ");
          Serial.println(pos2_12);
          while (pos2_12 > pos2_1 || pos2_22 > pos2_2) {
            if (pos2_2 >= 0 & pos2_2 <= 1600){
              pos2_2 += 1;
              digitalWrite(8, 1);
            }
            if (pos2_1 >= 0 & pos2_1 <= 1600){
              pos2_1 += 1;
              digitalWrite(17, 1);
            }
            delay(15);
            digitalWrite(17, 0);
            digitalWrite(8, 0);
          }
        }
        if (pos2_12 < pos2_1) {
          digitalWrite(16, 0);
          digitalWrite(18, 0);
          Serial.print(pos2_1);
          Serial.print("  ");
          Serial.println(pos2_12);
          while (pos2_12 < pos2_1 || pos2_22 < pos2_2) {
            if (pos2_2 >= 0 & pos2_2 <= 1600){
              pos2_2 -= 1;
              digitalWrite(8, 1);
            }
            if (pos2_1 >= 0 & pos2_1 <= 1600){
              pos2_1 -= 1;
              digitalWrite(17, 1);
            }
            delay(15);
            digitalWrite(17, 0);
            digitalWrite(8, 0);
          }
        }
        count1 = 0;
        count2 = 0;
        pos2_1 = constrain(pos2_1, 0, 1600);
        pos2_2 = constrain(pos2_2, 0, 1600);
        if (digitalRead(39)) {
          endd = 1;
          while (digitalRead(39)) {delay(10);}
          Serial.println(4);
        }
        if (digitalRead(37)) {
          endd = 2;
          while (digitalRead(37)) {delay(10);}
          Serial.println(2);
        }
        if (digitalRead(35)) {
          endd = 3;
          while (digitalRead(35)) {delay(10);}
          Serial.println(3);
        }
      }
    }
    if (regim == 2) {
      digitalWrite(20, 1);
      digitalWrite(3, 0);
      digitalWrite(46, 1);
      digitalWrite(10, 1);
      endd = 0;
      count1 = 0;
      count2 = 0;
      while (endd == 0) {
        t = 0;
        while (t <= 1000) {
          if (digitalRead(39)) {
            endd = 1;
            while (digitalRead(39)) {delay(10);}
            Serial.println(4);
          }
          if (digitalRead(37)) {
            endd = 2;
            while (digitalRead(37)) {delay(10);}
            Serial.println(2);
          }
          if (digitalRead(35)) {
            endd = 3;
            while (digitalRead(35)) {delay(10);}
            Serial.println(3);
          }
          delay(1);
          t += 1;
        }
        pos2_12 = constrain(pos2_1 + count1 * 64, 0, 1600);

        if (pos2_12 > pos2_1) {
          digitalWrite(16, 1);
          while (pos2_12 > pos2_1) {
            if (pos2_1 >= 0 & pos2_1 <= 1600){
              pos2_1 += 1;
              digitalWrite(17, 1);
            }
            delay(15);
            digitalWrite(17, 0);
          }
        }
        if (pos2_12 < pos2_1) {
          digitalWrite(16, 0);
          while (pos2_12 < pos2_1) {
            if (pos2_1 >= 0 & pos2_1 <= 1600){
              pos2_1 -= 1;
              digitalWrite(17, 1);
            }
            delay(15);
            digitalWrite(17, 0);
          }
        }
        count1 = 0;
        count2 = 0;
        pos2_1 = constrain(pos2_1, 0, 1600);
        if (digitalRead(39)) {
          endd = 1;
          while (digitalRead(39)) {delay(10);}
          Serial.println(4);
        }
        if (digitalRead(37)) {
          endd = 2;
          while (digitalRead(37)) {delay(10);}
          Serial.println(2);
        }
        if (digitalRead(35)) {
          endd = 3;
          while (digitalRead(35)) {delay(10);}
          Serial.println(3);
        }
      }
    }
    if (regim == 3) {
      Serial.println(12345);
      digitalWrite(20, 1);
      digitalWrite(3, 0);
      digitalWrite(46, 0);
      digitalWrite(10, 0);
      endd = 0;
      count1 = 0;
      count2 = 0;
      while (endd == 0) {
        t = 0;
        while (t <= 1000) {
          if (digitalRead(39)) {
            endd = 1;
            while (digitalRead(39)) {delay(10);}
            Serial.println(4);
          }
          if (digitalRead(37)) {
            endd = 2;
            while (digitalRead(37)) {delay(10);}
            Serial.println(2);
          }
          if (digitalRead(35)) {
            endd = 3;
            while (digitalRead(35)) {delay(10);}
            Serial.println(3);
          }
          delay(1);
          t += 1;
        }
        pos2_22 = constrain(pos2_2 + count1 * 64, 0, 1600);

        if (pos2_22 > pos2_2) {
          digitalWrite(18, 1);
          while (pos2_22 > pos2_2 & endd == 0) {
            if (pos2_2 >= 0 & pos2_2 <= 1600){
              pos2_2 += 1;
              digitalWrite(8, 1);
            }
            delay(15);
            digitalWrite(8, 0);
          }
        }
        if (pos2_22 < pos2_2) {
          digitalWrite(18, 0);
          while (pos2_22 < pos2_2 & endd == 0) {
            if (pos2_2 >= 0 & pos2_2 <= 1600){
              pos2_2 -= 1;
              digitalWrite(8, 1);
            }
            delay(15);
            digitalWrite(8, 0);
          }
        }
        count1 = 0;
        count2 = 0;
        pos2_2 = constrain(pos2_2, 0, 1600);
        if (digitalRead(39)) {
          endd = 1;
          while (digitalRead(39)) {delay(10);}
          Serial.println(4);
        }
        if (digitalRead(37)) {
          endd = 2;
          while (digitalRead(37)) {delay(10);}
          Serial.println(2);
        }
        if (digitalRead(35)) {
          endd = 3;
          while (digitalRead(35)) {delay(10);}
          Serial.println(3);
        }
      }
    }
    if (regim == 4) {
      Serial.println(123);
      digitalWrite(20, 1);
      digitalWrite(3, 0);
      digitalWrite(46, 0);
      digitalWrite(10, 01);
      endd = 0;
      count1 = 0;
      count2 = 0;
      t = 0;

      


      pos2_12 = constrain(1600 - pos2_2, 0, 1600);

      Serial.print(pos2_12);
      Serial.print("  ");
      Serial.println(pos2_1);

      if (pos2_12 > pos2_1) {
        digitalWrite(16, 1);
        while (pos2_12 > pos2_1) {
          if (pos2_1 >= 0 & pos2_1 <= 1600){
            pos2_1 += 1;
            digitalWrite(17, 1);
          }
          if (t % 200 < 100) {
            digitalWrite(46, 0);
            digitalWrite(10, 1);
          }
          else {
            digitalWrite(46, 1);
            digitalWrite(10, 0);
          }
          delay(15);
          t += 15;
          digitalWrite(17, 0);
        }
      }
      if (pos2_12 < pos2_1) {
        digitalWrite(16, 0);
        while (pos2_12 < pos2_1) {
          if (pos2_1 >= 0 & pos2_1 <= 1600){
            pos2_1 -= 1;
            digitalWrite(17, 1);
          }
           if (t % 200 < 100) {
            digitalWrite(20, 0);
            digitalWrite(3, 1);
          }
          else {
            digitalWrite(20, 1);
            digitalWrite(3, 0);
          }
          delay(15);
          t += 15;
          digitalWrite(17, 0);
        }
      }
      count1 = 0;
      count2 = 0;
      pos2_1 = constrain(pos2_1, 0, 1600);
      t = 0;


      Serial.println(123);
      //pos2_1 = pos2_2;
      while (endd == 0) {
        while (t <= 1000) {
          if (t % 200 < 100) {
            digitalWrite(20, 0);
            digitalWrite(3, 1);
          }
          else {
            digitalWrite(20, 1);
            digitalWrite(3, 0);
          }
          t ++;
          delay(1);
        }
        pos2_22 = constrain(pos2_2 + count2 * 64, 0, 1600);

        if (pos2_22 > pos2_2) {
          digitalWrite(16, 0);
          digitalWrite(18, 1);
          while (pos2_22 > pos2_2) {
            if (pos2_2 >= 0 & pos2_2 <= 1600){
              pos2_1 -= 1;
              pos2_2 += 1;
              digitalWrite(17, 1);
              digitalWrite(8, 1);
            }
            if (t % 200 < 100) {
              digitalWrite(20, 0);
              digitalWrite(3, 1);
            }
            else {
              digitalWrite(20, 1);
              digitalWrite(3, 0);
            }
            delay(15);
            t += 15;
            digitalWrite(17, 0);
            digitalWrite(8, 0);
          }
        }
        if (pos2_22 < pos2_2) {
          digitalWrite(16, 1);
          digitalWrite(18, 0);
          Serial.print(pos2_1);
          Serial.print("  ");
          Serial.println(pos2_12);
          while (pos2_22 < pos2_2) {
            if (pos2_2 >= 0 & pos2_2 <= 1600){
              pos2_1 += 1;
              pos2_2 -= 1;
              digitalWrite(17, 1);
              digitalWrite(8, 1);
            }
            if (t % 200 < 100) {
              digitalWrite(20, 0);
              digitalWrite(3, 1);
            }
            else {
              digitalWrite(20, 1);
              digitalWrite(3, 0);
            }
            delay(15);
            t += 15;
            digitalWrite(17, 0);
            digitalWrite(8, 0);
          }
        }
        t = 0;
        count1 = 0;
        count2 = 0;
        pos2_1 = constrain(pos2_1, 0, 1600);
        pos2_2 = constrain(pos2_2, 0, 1600);
        if (digitalRead(39)) {
          endd = 1;
          while (digitalRead(39)) {delay(10);}
          Serial.println(4);
        }
        if (digitalRead(37)) {
          endd = 2;
          while (digitalRead(37)) {delay(10);}
          Serial.println(2);
        }
        if (digitalRead(35)) {
          endd = 3;
          while (digitalRead(35)) {delay(10);}
          Serial.println(3);
        }
      }
    }
    
    if (endd == 2) {
      regim += 1; 
      if (regim > 4) {regim = 1;}; 
      endd = 0;
    }
    if (endd == 1) {sermot = 1; regim = 1; endd = 0;}
  }
  if (endd == 3) {
    endd = 0;
    servo1.write(90);
    servo2.write(90);
    pos2_12 = constrain(800, 0, 1600);

    if (pos2_12 > pos2_1) {
      digitalWrite(16, 1);
      while (pos2_12 > pos2_1) {
        if (pos2_1 >= 0 & pos2_1 <= 1600){
          pos2_1 += 1;
          digitalWrite(17, 1);
        }
        delay(15);
        digitalWrite(17, 0);
      }
    }
    if (pos2_12 < pos2_1) {
      digitalWrite(16, 0);
      while (pos2_12 < pos2_1) {
        if (pos2_1 >= 0 & pos2_1 <= 1600){
          pos2_1 -= 1;
          digitalWrite(17, 1);
        }
        delay(15);
        digitalWrite(17, 0);
      }
    }
    pos2_22 = constrain(800, 0, 1600);
    if (pos2_22 > pos2_2) {
      digitalWrite(18, 1);
      while (pos2_22 > pos2_2) {
        if (pos2_2 >= 0 & pos2_2 <= 1600){
          pos2_2 += 1;
          digitalWrite(8, 1);
        }
        delay(15);
        digitalWrite(8, 0);
      }
    }
    if (pos2_22 < pos2_2) {
      digitalWrite(18, 0);
      while (pos2_22 < pos2_2) {
        if (pos2_2 >= 0 & pos2_2 <= 1600){
          pos2_2 -= 1;
          digitalWrite(8, 1);
        }
        delay(15);
        digitalWrite(8, 0);
      }
    }
    pos2_1 = 800;
    pos2_2 = 800;
    count1 = 0;
    e = count1;
    //Serial.println(1584);
    while (digitalRead(4)) {}
    //Serial.println(1583);
    count1 = 0;
    count2 = 0;
  }
}