#include <Servo.h>

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;
int pos;
int tunda = 15;
int button = 13;
void setup() {
  servo1.attach(3);
  servo2.attach(5);
  servo3.attach(6);
  servo4.attach(9);
  servo5.attach(10);
  servo6.attach(11);
  awal();
  pinMode(button, INPUT);
  digitalWrite(button, HIGH);
}

void loop() {
  if(digitalRead(button)==0){
    digitalWrite(12, LOW);
    digitalWrite(8, LOW);
    digitalWrite(7, LOW);
    ambil1();
    digitalWrite(12, HIGH);
    ambil2();
    digitalWrite(8, HIGH);
    ambil3();
    digitalWrite(7, HIGH);
    digitalWrite(button, HIGH);
  }else{
    awal();
  }  
 }
 void awal()
 {
  servo1.write(10);
  servo2.write(90);
  servo3.write(65);
  servo4.write(75);
  servo5.write(130);
  servo6.write(90);
}
 void ambil1()
 {
  for (pos = 90; pos >= 45; pos -= 1) { 
    servo2.write(pos);              
    delay(30);
  }for (pos = 90; pos >= 40; pos -= 1) { 
    servo6.write(pos);              
    delay(40);
  }    
  for (pos = 130; pos >= 70; pos -= 1) { //turun
    servo5.write(pos);              
    delay(50);
  }
  delay(1000);
  for (pos = 10; pos <= 40; pos += 1) { //ambil jepit
    servo1.write(pos);              
    delay(40);
  }  
  delay(1000);
for (pos = 70; pos <= 130; pos += 1) { //naik
    servo5.write(pos);              
    delay(40);
}
for (pos = 40; pos <= 140; pos ++) { 
    servo6.write(pos);              
    delay(40);
}
for (pos = 130; pos >= 75; pos -= 1) { 
    servo5.write(pos);              
    delay(30);
}  
delay(1000);
for (pos = 40; pos >= 10; pos -= 1) { //lepas
    servo1.write(pos);              
    delay(30);
}  
delay(1000);
for (pos = 75; pos <= 130; pos ++) { //naik
    servo5.write(pos);              
    delay(40);
 }
 }
 void ambil2()
 {
    for (pos = 180; pos >= 90; pos -= 1) {
    servo1.write(pos);
    delay(tunda);}
    for (pos = 90; pos >= 60; pos -= 1) {
    servo2.write(pos);
    delay(tunda);}
    for (pos = 120; pos >= 100; pos -= 1) {
    servo3.write(pos);
    delay(tunda);}
    for (pos = 40; pos >= 10; pos -= 1) {
    servo6.write(pos);
    delay(tunda);}
    for (pos = 10; pos <= 40; pos += 1) {
    servo6.write(pos);}
 }
 void ambil3()
 {
    for (pos = 90; pos <= 180; pos += 1) {
    servo1.write(pos);
    delay(tunda);}
    for (pos = 100; pos >= 90; pos -= 1) {
    servo2.write(pos);
    delay(tunda);}
    for (pos = 160; pos >= 90; pos -= 1) {
    servo3.write(pos);
    delay(tunda);}
    for (pos = 60; pos <= 40; pos += 1) {
    servo6.write(pos);
    delay(tunda);}
 }