#include <Servo.h>

Servo ServoBase;
Servo ServoArmOne;
Servo ServoArmTwo;
const int SW_pin = 10;
const int X_pin = A1;
const int Y_pin = A0;
const int podpin = A3;
const int leftButton = 7;
const int rightButton = 8;

void setup () {
  ServoBase.attach(13);
  ServoArmOne.attach(12);
  ServoArmTwo.attach(9);
  Serial.begin(9600);
  pinMode(SW_pin, INPUT);
  digitalWrite(SW_pin, HIGH);
  pinMode(leftButton, INPUT);
  pinMode(rightButton, INPUT);


}

void loop() {

bool SW = digitalRead(SW_pin);
if (!SW) {
  ServoBase.write(90);
  ServoArmOne.write(90);
}



  //base movement
int Y_State = analogRead(Y_pin);
int currentYState = ServoBase.read();



  if (Y_State > 524) {
    int newYState = currentYState + 1;
    ServoBase.write(newYState);
    Serial.print("arm base movement");
    Serial.print("\n");
    delay(15);
  }else if (Y_State < 502) {
    int newYState = currentYState - 1;
    ServoBase.write(newYState);
    Serial.print("arm base movement");
    Serial.print("\n");
    delay(15);
  }

//arm movement

int X_State = analogRead(X_pin);
int currentXState = ServoArmOne.read();


  if (X_State > 524) {
    int newXState = currentXState + 1;
    ServoArmOne.write(newXState);
    Serial.print("arm one movement");
    Serial.print("\n");
    delay(15);

  }else if (X_State < 502) {
    int newXState = currentXState - 1;
    ServoArmOne.write(newXState);
    Serial.print("arm one movement");
    Serial.print("\n");
    delay(15);
  }


//armTwo movement

int currentStateLeftButton = digitalRead(leftButton);
int currentStateRightButton = digitalRead(rightButton);
int currentStateArmTwo = ServoArmTwo.read();
/*Serial.print("button: ");
Serial.print(currentStateLeftButton);
Serial.print(currentStateRightButton);
Serial.print("\n");*/

if (currentStateLeftButton == HIGH) {
  Serial.print("test");

  int newStateArmTwo = currentStateArmTwo + 1;
  ServoArmTwo.write(newStateArmTwo);
  delay(15);
}

if (currentStateRightButton == HIGH) {
    int newStateArmTwo = currentStateArmTwo - 1;
    ServoArmTwo.write(newStateArmTwo);
    delay(15);
}

}