#include <Servo.h>

Servo escleft;
Servo escright;
int val;
int encoder0PinA = 3;
int encoder0PinB = 4;
int encoder0Pos = 0;
int encoder0PinALast = LOW;
int n = LOW;
void setup() {

pinMode (encoder0PinA,INPUT);
pinMode (encoder0PinB,INPUT);
Serial.begin (9600);
escright.attach(9);
escleft.attach(10);
escright.write(90);
escleft.write(90);
delay(4200); //arms ESC
}
void loop(){
encoder0Pos = encoderfunction();
while(encoder0Pos < 4000){
  n = digitalRead(encoder0PinA);
   if ((encoder0PinALast == LOW) && (n == HIGH)) {
     if (digitalRead(encoder0PinB) == LOW) {
       encoder0Pos--;
     } else {
       encoder0Pos++;
     }
     Serial.print (encoder0Pos);
   }
   encoder0PinALast = n;
escleft.writeMicroseconds(1450);//give motors throttle
escright.writeMicroseconds(1550);//
}
escleft.writeMicroseconds(1500);//brakes both motors
escright.writeMicroseconds(1500);
}
int encoderfunction(){

 n = digitalRead(encoder0PinA);
   if ((encoder0PinALast == LOW) && (n == HIGH)) {
     if (digitalRead(encoder0PinB) == LOW) {
       encoder0Pos--;
     } else {
       encoder0Pos++;
     }
     Serial.print (encoder0Pos);
   }
    return encoder0Pos;
    encoder0PinALast = n;
}