#define PB1 12
#define PB2 13

#include<Servo.h>
Servo Servo1;
Servo Servo2;
Servo Servo3;
Servo Servo4;
Servo Servo5;
Servo Servo6;
int kondisi=0;

void setup() {
  // put your setup code here, to run once:
  Servo1.attach(3);
  Servo2.attach(5);
  Servo3.attach(6);
  Servo4.attach(9);
  Servo5.attach(10);
  Servo6.attach(11);
  
  int S1a=90; int S1b = 90;
  int S2a=90; int S2b = 90;  
  int S3a=90; int S3b = 90;  
  int S4a=90; int S4b = 90;  
  int S5a=90; int S5b = 90;  
  int S6a=90; int S6b = 90;  

  Servo1.write(0);
  Servo2.write(10);
  Servo3.write(20);
  Servo4.write(30);
  Servo5.write(40);
  Servo6.write(50);

  pinMode(PB1, INPUT_PULLUP);
  pinMode(PB2, INPUT_PULLUP);

}

void myServo1 (int S1a, int S1b) {
  if(S1a<S1b) {
    for(S1a=S1a; S1a<S1b; S1a++) {
      Servo1.write(S1a);
      delay(30);
    }
  }
  else if(S1a>S1b) {
    for(S1a=S1a; S1a>S1b; S1a--) {
      Servo1.write(S1a);
      delay(30);
    }
  }
}

void myServo2 (int S2a, int S2b) {
  if(S2a<S2b) {
    for(S2a=S2a; S2a<S2b; S2a++) {
      Servo2.write(S2a);
      delay(30);
    }
  }
  else if(S2a>S2b) {
    for(S2a=S2a; S2a>S2b; S2a--) {
      Servo2.write(S2a);
      delay(30);
    }
  }
}

void myServo3 (int S3a, int S3b) {
  if(S3a<S3b) {
    for(S3a=S3a; S3a<S3b; S3a++) {
      Servo3.write(S3a);
      delay(30);
    }
  }
  else if(S3a>S3b) {
    for(S3a=S3a; S3a>S3b; S3a--) {
      Servo3.write(S3a);
      delay(30);
    }
  }
}

void myServo4 (int S4a, int S4b) {
  if(S4a<S4b) {
    for(S4a=S4a; S4a<S4b; S4a++) {
      Servo4.write(S4a);
      delay(30);
    }
  }
  else if(S4a>S4b) {
    for(S4a=S4a; S4a>S4b; S4a--) {
      Servo4.write(S4a);
      delay(30);
    }
  }
}

void myServo5 (int S5a, int S5b) {
  if(S5a<S5b) {
    for(S5a=S5a; S5a<S5b; S5a++) {
      Servo5.write(S5a);
      delay(30);
    }
  }
  else if(S5a>S5b) {
    for(S5a=S5a; S5a>S5b; S5a--) {
      Servo5.write(S5a);
      delay(30);
    }
  }
}

void myServo6 (int S6a, int S6b) {
  if(S6a<S6b) {
    for(S6a=S6a; S6a<S6b; S6a++) {
      Servo6.write(S6a);
      delay(30);
    }
  }
  else if(S6a>S6b) {
    for(S6a=S6a; S6a>S6b; S6a--) {
      Servo6.write(S6a);
      delay(30);
    }
  }
}

void loop() {
  // put your main code here, to run repeatedly:
  if(digitalRead(PB1)==0){
    kondisi=1;
  }
  if(digitalRead(PB2)==0){
    kondisi=2;
  }
  
  //Posisi Default
  if(kondisi==1){
    delay(1000);
    myServo4(90,125);
    myServo3(90,130);
    myServo4(125,160);
    myServo2(90,135);
    myServo3(130,170);
    myServo5(90,95);
    kondisi++;
    delay(3000);
  }
  
  //Ambil workpiece
  if(kondisi==2){
    myServo1(90,30);
    myServo6(90,30);
    myServo5(90,0);
    myServo2(135,90);
    myServo4(160,140);
    myServo2(90,60);
    myServo3(170,180);
    myServo2(60,50);
    delay(2000);
    myServo6(30,55);
    delay(1000);
    kondisi++;
  }
    
  //Angkat geser
  if(kondisi==3){
    myServo2(50,90);  
    myServo4(140,180);
    myServo1(30,150);
    kondisi++;
  }

  //Simpan workpiece
  if(kondisi==4){
    myServo4(180,140);  
    myServo2(90,50);
    myServo6(55,30);
    delay(1000);
    myServo3(180,170);
    myServo2(50,90);
    myServo5(0,95);
    myServo6(30,90);
    kondisi++;
  }  

  //Kembali ke Default
  if(kondisi==5){
    myServo4(140,160);
    myServo2(90,135);
    myServo1(150,90);
    kondisi++;
  }
    
}