#include <Servo.h>
int flag = 0;
int sensorPIN = A0;
int sensor_status = 0;
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;

int pos1 = 120;
int pos2 = 60;
int pos3 = 60;
int pos4 = 60;
int pos5 = 60;

//1: 30-120 = 90
//2: 60-150 = 90
//3: 60-150 = 90
//4: 60-150 = 90
//5: 60-150 = 90

void setup() {
  Serial.begin(9600);
  servo1.attach(3);
  servo2.attach(5);
  servo3.attach(6);
  servo4.attach(9);
  servo5.attach(10);
  servo1.write(pos1);
  servo2.write(pos2);
  servo3.write(pos3);
  servo4.write(pos4);
  servo5.write(pos5);
  pinMode(sensorPIN, INPUT);
  delay(500);
}

void loop() {
  sensor_status = analogRead(sensorPIN);
  Serial.println(sensor_status);
  if (sensor_status > 960 && flag == 0) {
    all_fingers_open();
    flag = 1;
  }
  else if (sensor_status < 960 && flag == 1) {
    all_fingers_close();
    flag = 0;
  }
  /*
    finger1open();
    finger2open();
    finger3open();
    finger4open();
    finger5open();
    delay(5000);
    finger1close();
    finger2close();
    finger3close();
    finger4close();
    finger5close();
    delay(5000);
    all_fingers_open();
    delay(5000);
    all_fingers_close();
    delay(5000);
  */

}
void all_fingers_open() {
  for (pos1 = 120, pos2 = 60; pos1 >= 30, pos2 <= 150; pos1 -= 1, pos2 += 1) {
    servo1.write(pos1);
    servo2.write(pos2);
    servo3.write(pos2);
    servo4.write(pos2);
    servo5.write(pos2);
    delay(15);
  }
}
void all_fingers_close() {
  for (pos1 = 30, pos2 = 150; pos1 <= 120, pos2 >= 60; pos1 += 1, pos2 -= 1) {
    servo1.write(pos1);
    servo2.write(pos2);
    servo3.write(pos2);
    servo4.write(pos2);
    servo5.write(pos2);
    delay(15);
  }
}
void finger1open() {
  for (pos1 = 120; pos1 >= 30; pos1 -= 1) {
    servo1.write(pos1);
    delay(15);
  }
}
void finger1close() {
  for (pos1 = 30; pos1 <= 120; pos1 += 1) {
    servo1.write(pos1);
    delay(15);
  }
}

void finger2open() {
  for (pos2 = 60; pos2 <= 150; pos2 += 1) {
    servo2.write(pos2);
    delay(15);
  }
}
void finger2close() {
  for (pos2 = 150; pos2 >= 60; pos2 -= 1) {
    servo2.write(pos2);
    delay(15);
  }
}

void finger3open() {
  for (pos3 = 60; pos3 <= 150; pos3 += 1) {
    servo3.write(pos3);
    delay(15);
  }
}
void finger3close() {
  for (pos3 = 150; pos3 >= 60; pos3 -= 1) {
    servo3.write(pos3);
    delay(15);
  }
}

void finger4open() {
  for (pos4 = 60; pos4 <= 150; pos4 += 1) {
    servo4.write(pos4);
    delay(15);
  }
}
void finger4close() {
  for (pos4 = 150; pos4 >= 60; pos4 -= 1) {
    servo4.write(pos4);
    delay(15);
  }
}

void finger5open() {
  for (pos5 = 60; pos5 <= 150; pos5 += 1) {
    servo5.write(pos5);
    delay(15);
  }
}
void finger5close() {
  for (pos5 = 150; pos5 >= 60; pos5 -= 1) {
    servo5.write(pos5);
    delay(15);
  }
}