#include <Servo.h>

Servo servos[4];        // Create servo object

const int Gripper = A0; // Potentiometer control grip
const int sGrip = 3; // PWM pin for the servo

const int BaseY = A2; //Base control
const int servoBase = 5;//PWM pin for servo

const int Elbow = A3; // Elbow
const int sElbow = 9; // PWM pin for servo

const int Button = 12; //Button toggle on/off

const int sSwitch = 10; // PWM wrist

const int PINS[4] = {sGrip,servoBase,sElbow,sSwitch};

void setup() {

  pinMode(Button, INPUT_PULLUP);
  //bool ButtonState = false;
  servos[0].attach(sGrip);
  servos[1].attach(servoBase);
  servos[2].attach(sElbow);
  servos[3].attach(sSwitch);
  for(int i=0;i<4;i++){
    servos[i].write(90);
  }

}

void loop() {
  int pot = analogRead(Gripper);
  int angle_grip = map(pot,0,1023,0,180);
  servos[0].write(angle_grip); //Grip tightness

  int base = analogRead(BaseY);
  int base_angle = map(base,0,1023,0,180);
  servos[1].write(base_angle);//Control the angle of the base

  if(digitalRead(Button) == LOW){
    int wrist = analogRead(Elbow);
    int wrist_angle = map(wrist,0,1023,0,180);
    servos[3].write(wrist_angle);//wrist control
    unsigned long now = millis();
  }
  else{
    int elbow = analogRead(Elbow);
    int elbow_angle = map(elbow,0,1023,0,180);
    servos[2].write(elbow_angle); // Elbow angle
}
}