#include <Servo.h>

Servo grip;       //집게
Servo bottom;    //회전부  
Servo arm1;     //관절부1
Servo arm2;    //관절부2  

void setup() {
  Serial.begin(9600);

  grip.attach(3);        //집게  
  bottom.attach(5);     //회전부 
  arm1.attach(7);      //관절부1
  arm2.attach(11);    //관절부2  

grip.write(150);
bottom.write(110);
arm1.write(150);
arm2.write(90);
delay(1000);
  }

void loop() {
 int val=analogRead(A0);
 delay(500);

 if(val<200){                   //흰색 병뚜껑 감지했을 때
  
  white();
 }
  
     


else if (val>850){
  black();
}

else{
  first();
}
}


void first(){
grip.write(150);
bottom.write(110);
arm1.write(150);
arm2.write(90);
delay(1000);
}

void black(){
  
  // 암2 내리기
  for(pos = 90; pos >= 55; pos -= 1) {
   arm2.write(pos);
   delay(15);
   }
   delay(1000);

   // 집는다
   for(pos = 150; pos >= 95; pos -= 1) {
   grip.write(pos);
   delay(15);
  }
   delay(1000);
  }
  
  //올라온다.
  for(pos = 55; pos <=90; pos += 1) {
  arm2.write(pos);
  delay(15);
   }
  delay(1000);
}

  //왼쪽으로
  for(pos = 110; pos <= 180; pos += 1) {
  bottom.write(pos);
  delay(15);
  }
  delay(1000);

  // 아래로 이동
  for(pos = 90; pos >= 55; pos -= 1) {
  arm2.write(pos);
  delay(15);
  }
  delay(1000);

  //집게 풀기
  for(pos = 95; pos <= 150; pos += 1) {
  grip.write(pos);
  delay(15);
  }
  delay(1000);

   // 위로 올라오기
  for(pos = 60; pos <= 90; pos += 1) {
  arm2.write(pos);
  delay(15);
  }
  delay(1000);  

  // 원위치 (왼쪽에서 원래 위치로)
  for(pos = 180; pos >= 110; pos -= 1) {
  bottom.write(pos);
  delay(15);
  }
  delay(1000);
  
  }


void white(){

  // 암2 내리기
  for(pos = 90; pos >= 55; pos -= 1) {
   arm2.write(pos);
   delay(15);
   }
   delay(1000);

   // 집는다
   for(pos = 150; pos >= 95; pos -= 1) {
   grip.write(pos);
   delay(15);
  }
   delay(1000);
  }
  
  //올라온다.
  for(pos = 55; pos <=90; pos += 1) {
  arm2.write(pos);
  delay(15);
   }
  delay(1000);
}

   //오른쪽으로 이동
  for(pos = 110; pos >= 15; pos -= 1) {
  bottom.write(pos);
  delay(15);
  }
  delay(1000);

// 암2 아래로 이동
  for(pos = 90; pos >= 60; pos -= 1) {
  arm2.write(pos);
  delay(15);
  }
  delay(1000);

  // 집게 풀기
  for(pos = 95; pos <= 150; pos += 1) {
  grip.write(pos);
  delay(15);
  }
  delay(1000);

  // 위로 올라오기
  for(pos = 60; pos <= 90; pos += 1) {
  arm2.write(pos);
  delay(15);
  }
  delay(1000);

  // 원 위치(오른쪽에서 원 위치로)
  for(pos = 15; pos <= 110; pos += 1) {
  bottom.write(pos);
  delay(15);
  }
  delay(1000);
}