#include <Servo.h>
//Potentiometer
const int Pot1 = 33;
const int Pot2 = 13;
const int Pot3 = 27;
//Servo Motor
const int sPin1 = 21;
const int sPin2 = 19; 
const int sPin3 = 18;
const int sPin4 = 25;
//Joystick
const int yPin = 35;

Servo sv1; // Joint 1
Servo sv2; // Joint 2
Servo sv3; // Joint 3
Servo sv4; // Gripper

int aRead;

int serPos1 = 0;
int serPos2 = 0;
int serPos3 = 0;
int serPos4 = 0;

int t = 1000;

void setup() {
  // put your setup code here, to run once:
  pinMode(Pot1, INPUT);
  pinMode(Pot2, INPUT);
  pinMode(Pot3, INPUT);
  pinMode(yPin, INPUT);
  
  pinMode(sPin1, OUTPUT);
  pinMode(sPin2, OUTPUT);
  pinMode(sPin3, OUTPUT);
  pinMode(sPin4, OUTPUT);
  
  sv1.attach(sPin1);
  sv2.attach(sPin2);
  sv3.attach(sPin3);
  sv4.attach(sPin4);

  Serial.begin(9600);
}

void loop() {
  // put your main code here, to run repeatedly:
  // aRead to serPos
 
  //Pot1 and sPin1
  aRead = analogRead(Pot1);
  serPos1 = (180./4095.)*aRead;
  sv1.write(serPos1);
  
  //Pot2 and sPin2
  aRead = analogRead(Pot2);
  serPos2 = (180./4095.)*aRead;
  sv2.write(serPos2);
 
  //Pot3 and sPin3
  aRead = analogRead(Pot3);
  serPos3 = (180./4095.)*aRead;
  sv3.write(serPos3);

  //Joystick
  aRead = analogRead(yPin);
  serPos4 = (180./4095.)*aRead;
  sv4.write(serPos4);

  // Display Position on Monitor
  Serial.print("1st Motor: ");
  Serial.print(serPos1);
  Serial.print(" deg");
  Serial.println();
  
  Serial.print("2nd Motor: ");
  Serial.print(serPos2);
  Serial.print(" deg");
  Serial.println();
  
  Serial.print("3rd Motor: ");
  Serial.print(serPos3);
  Serial.print(" deg");
  Serial.println();
  
   if(serPos4 == 0 || serPos4 >= 180)
  {
    Serial.print("Gripper Open");
  }
  else
  {
    Serial.print("Gripper Close");
  }
  Serial.println();
  delay(t);
}