//#include <VarSpeedServo.h>
#include <Servo.h>
#define basePin       10  // Servo pins
#define forwardPin    9
#define upPin         8
#define gripPin       7

#define X_AXIS        A0  // Joystick pins
#define Y_AXIS        A1
#define SW_GRIP       A2

#define LED_RED       13  // Indictors
#define LED_GREEN     12
#define LED_YELLOW    11

int grip_open  = 30;    // Grip Servo open angle
int grip_close = 120;   // Grip Servo close angle
int x_Angle, y_Angle;   // Joystick angles

int prevState = LOW;

Servo servo_base, servo_forward, servo_up, servo_grip; // Sevo objects

void setup() {

  Serial.begin(9600);
  pinMode(SW_GRIP, INPUT_PULLUP);
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_YELLOW, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);

  servo_base.attach(basePin);
  servo_forward.attach(forwardPin);
  servo_up.attach(upPin); 
  servo_grip.attach(gripPin);

  // Initialize the arm
  servo_base.write(90);
  servo_forward.write(90);
  servo_up.write(90);
  servo_grip.write(90);
}

void loop() {
  readSwitch();
  x_Angle = xValue();   // Get the x-angle from joystick
  //y_Angle = yValue();   // Get the y-angle from joystick
  servo_base.write(x_Angle);//, 120, true); // write x_Angle, speed, wait until finish
  delay(100);
  servo_forward.write(y_Angle);
  delay(100);
}

// Function to read x-Axis
int xValue(){
  int x = analogRead(X_AXIS);
  Serial.print("X_VALUE = ");
  Serial.println(x);

  int angleX = map(x, 0,1024,0,180);
  return angleX;
}

// Function to read y-Axis
int yValue(){
  int y = analogRead(Y_AXIS);
  Serial.print("Y_VALUE = ");
  Serial.println(y);
  int angleY = map(y, 0,1024,0,180);
  return angleY;
}

void readSwitch(){
   int currState = digitalRead(SW_GRIP);
   if(currState != prevState) {
      servo_grip.write(90);
   }else{
    deay(500);
      servo_grip.write(0);
   }
   prevState = currState;
}