#include <Servo.h>
Servo servo1;

int servoPosition;
int currentValue;
int lastValue=1;
int encoderPosition;
unsigned long now;
unsigned long past;
int buttonState;
int degreesToHome;
int j;
int xxx;

void setup() {
// set PIN and range in us. Standard servo range is 500 - 2500 us
servo1.attach(6, 500, 2500);

  pinMode(2, INPUT);
  pinMode(3, INPUT);
  pinMode(4, INPUT_PULLUP); //Button
  pinMode(5, OUTPUT);       //Yellow LED
  pinMode(7, OUTPUT);       //Red LED
  servo1.write(90);
  encoderPosition=90;

 Serial.begin(9600);
 Serial.println("BEGIN");
 Serial.println();

}

void loop() {

  currentValue = digitalRead(2);
   if (currentValue != lastValue){
     //Clockwise
     if (digitalRead(3) != currentValue){
      encoderPosition ++;
      if (encoderPosition>180) {
      encoderPosition=180;
      digitalWrite(7, HIGH);
      past=millis();
      }
       servo1.write(encoderPosition);
       lastValue = currentValue;
       Serial.println ("Clockwise");
       Serial.println(encoderPosition);
     } 
     //Counterclockwise
     else {
      encoderPosition--;
      if (encoderPosition<0) {
      encoderPosition=0;
      digitalWrite(7, HIGH);
      past=millis();
      }
       servo1.write(encoderPosition);
       lastValue = currentValue;
       Serial.println("Counterclockwise");
       Serial.println(encoderPosition);
       }
   } 

//Limit LED, delayed
now=millis();
if (now-past>5000) {
  digitalWrite(7, LOW);
  past=now;
}

//Homing
 buttonState = digitalRead(4);
 if (buttonState==LOW) {
 degreesToHome = encoderPosition - 90;
 digitalWrite(5, HIGH);
 Serial.println("degreesToHome");
 Serial.println(degreesToHome);


 if (degreesToHome<90) {
 for (j=1;j<=degreesToHome;j=j+1){
 servo1.write(encoderPosition --);
 delay(50);
 }
}

 if (degreesToHome<0) {
 for (j=1;j>=degreesToHome;j=j-1){
 servo1.write(encoderPosition ++);
 delay(50);
 }
}
digitalWrite(5, LOW);
}

}