#include <Servo.h>
#include <Encoder.h>
// Rotary Encoder Inputs
#define CLK 2
#define DT 3

Encoder Enc(CLK, DT);

Servo servo1;
Servo servo2;
int counter = 90;
int steps = 10;
int currentStateCLK;
int lastStateCLK;
const int SW_PIN = 4;


void setup() {
  // Set encoder pins as inputs
  pinMode(CLK,INPUT);
  pinMode(DT,INPUT);
  pinMode(SW_PIN, INPUT);
  digitalWrite(SW_PIN, HIGH);
  //attachInterrupt (0,isr,FALLING);
  // Setup Serial Monitor
  Serial.begin(9600);
  // Attach servo on pin 9 to the servo object
  servo1.attach(9);
  servo2.attach(10);
  servo1.write(counter);
  servo2.write(counter);
  // Read the initial state of CLK
  lastStateCLK = digitalRead(CLK);
}
void loop() {
  // Read the current state of CLK
  currentStateCLK = digitalRead(CLK);
  // If last and current state of CLK are different, then pulse occurred
  // React to only 1 state change to avoid double count
  if (currentStateCLK != lastStateCLK  && currentStateCLK % 2 == 0){
    // If the DT state is different than the CLK state then
    // the encoder is rotating CCW so decrement
    if (digitalRead(DT) != currentStateCLK) {
      counter -= steps;
      if (counter<0)
        servo1.write(0);
        servo2.write(0);
    } else {
      // Encoder is rotating CW so increment
      counter += steps;
      if (counter>190)
        servo1.write(190);
        servo2.write(190);
    }
    // Move the servo
    servo1.write(counter);
    servo2.write(counter);
    Serial.print("Position: ");
    Serial.println(counter);

  }
  // Remember last CLK state
  lastStateCLK = currentStateCLK;
  //delay(1000);
  if (digitalRead(SW_PIN) == LOW)
  {
    counter = 90;
    servo1.write(90);
    servo2.write(90);
  }
}