#include <Servo.h>

Servo firServo;
Servo secServo;
Servo thiServo;
Servo forServo;

int firServoPos;
int secServoPos;
int thiServoPos;
int forServoPos;

int lastClk1 = HIGH;
int lastClk2 = HIGH;

void setup() {
  Serial.begin(9600);
  firServo.attach(3);
  secServo.attach(5);
  thiServo.attach(6);
  forServo.attach(9);
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(7, INPUT);
  pinMode(8, INPUT);
  pinMode(10, INPUT);
  pinMode(11, INPUT);
}

void loop() {

  firServoPos = firServo.read();
  secServoPos = secServo.read();
  thiServoPos = thiServo.read();
  forServoPos = forServo.read();


  
  firServo.write(firServoPos + map(analogRead(A0), 0, 1023, -1, 1));
  secServo.write(secServoPos + map(analogRead(A1), 0, 1023, -1, 1));


  int newClk1 = digitalRead(7);
  if (newClk1 != lastClk1) {
    // There was a change on the CLK pin
    lastClk1 = newClk1;
    int dtValue1 = digitalRead(8);
    if (newClk1 == LOW && dtValue1 == HIGH) {
      thiServoPos -= 5;
      thiServo.write(thiServoPos);
    }
    if (newClk1 == LOW && dtValue1 == LOW) {
      thiServoPos += 5;
      thiServo.write(thiServoPos);
    }
  }
  int newClk2 = digitalRead(10);
  if (newClk2 != lastClk2) {
    // There was a change on the CLK pin
    lastClk2 = newClk2;
    int dtValue2 = digitalRead(11);
    if (newClk2 == LOW && dtValue2 == HIGH) {
      forServoPos -= 5;
      forServo.write(forServoPos);
    }
    if (newClk2 == LOW && dtValue2 == LOW) {
      forServoPos += 5;
      forServo.write(forServoPos);
    }
  }
  
  delay(10);
  


}