#include <Servo.h>

Servo servo_0; // hand
Servo servo_1; // arm1
Servo servo_2; // arm2
Servo servo_3; // pan

int val0, val1, val2, val3; // Manipulator Potension meter value
int scv0, scv1, scv2, scv3; // Servo control value (Servo angle)

void setup() {
  Serial.begin(115200);
  servo_0.attach(13, 500, 2500); // hand
  servo_1.attach(12, 500, 2500); // arm1
  servo_2.attach(11, 500, 2500); // arm2
  servo_3.attach(10, 500, 2500); // pan
}

void loop() {
  if (digitalRead(17) == HIGH) {
    val0 = 825;
  } else {
    val0 = 1000;
  }  
  val1 = analogRead(28);
  val2 = analogRead(27);
  val3 = analogRead(26);

  Serial.print("val0 = "),  Serial.print(val0);
  Serial.print(", val1 = "),  Serial.print(val1);
  Serial.print(", val2 = "),  Serial.print(val2);
  Serial.print(", val3 = "),  Serial.println(val3);

  scv0 = map(val0, 0, 1023, 0, 180); // hand Manipulator
  scv1 = map(val1, 100, 800, 0, 180); // Arm1 Manipulator
  if (scv1 >= 180) {
    scv1 = 180;
  } else if (scv1 <= 0.0) {
    scv1 = 0.0;
  }
  scv2 = map(val2, 700, 125, 0, 180); // Arm2 Manipulator
  if (scv2 >= 180) {
    scv2 = 180;
  } else if (scv2 <= 0) {
    scv2 = 0;
  }
  scv3 = map(val3, 725, 55, 0, 180); //  pan Manipulator
  if (scv3 >= 180) {
    scv3 = 180;
  } else if (scv3 <= 0) {
    scv3 = 0;
  }
  
  servo_0.write(scv0); // Hand Servo
  servo_1.write(scv1); // Arm1 Servo
  servo_2.write(scv2); // Arm2 Servo
  servo_3.write(scv3); // Pan Servo

  Serial.print("scv0 = "),  Serial.print(scv0);
  Serial.print(", scv1 = "),  Serial.print(scv1);
  Serial.print(", scv2 = "),  Serial.print(scv2);
  Serial.print(", scv3 = "),  Serial.println(scv3);
}