#include <Servo.h>

Servo base;  //inisialisasi servo untuk base
Servo shoulder;  //inisialisasi servo untuk shoulder
Servo elbow;  //inisialisasi servo untuk elbow
Servo gripper; //inisialisasi servo untuk gripper

int pos1 = 90;  //posisi servo base
int pos2 = 60;  //posisi servo shoulder
int pos3 = 90;  //posisi servo elbow 
int pos4 = 0;  //posisi servo gripper

void setup() {
  base.attach(3);  //pin yang digunakan untuk servo base
  shoulder.attach(5); //pin yang digunakan untuk servo shoulder
  elbow.attach(6);  //pin yang digunakan untuk servo elbow
  gripper.attach(9);  //pin yang digunakan untuk servo gripper

  base.write(pos1);
  shoulder.write(pos2);
  elbow.write(pos3);
  gripper.write(pos4);
}

void loop() {

  delay(1000);
  // Ambil Barang
  //Base
  for (pos1; pos1 <= 180; pos1 += 1) { //perulangan untuk menggerakkan servo base ke 180 derajat
    base.write(pos1);
    delay(35);
  }
  //shoulder
  for (pos2; pos2 <= 120; pos2 += 1) { //perulangan untuk menggerakkan servo shoulder ke 120 derajat
    shoulder.write(pos2);
    delay(35);
  }
  //elbow
  for (pos3; pos3 <= 150; pos3 += 1) { //perulangan untuk menggerakkan servo elbow ke 150 derajat
    elbow.write(pos3);
    delay(35);
  }
  //gripper
  for (pos4; pos4 <= 180; pos4 += 1) { //perulangan untuk menggerakkan servo gripper ke 180 derajat
    gripper.write(pos4);
    delay(35);
  }
  delay(1000); //delay selama 1 detik

  // Pindah Barang
  //shoulder
  for (pos2; pos2 >= 60; pos2 -= 1) { //perulangan untuk menggerakkan servo shoulder ke 60 derajat
    shoulder.write(pos2);
    delay(35);
  }
  //elbow
  for (pos3; pos3 >= 60; pos3 -= 1) { //perulangan untuk menggerakkan servo elbow 60 derajat
    elbow.write(pos3);
    delay(35);
  }
  //Base
  for (pos1; pos1 >= 0; pos1 -= 1) { //perulangan untuk menggerakkan servo base 0 derajat
    base.write(pos1);
    delay(35);
  }
  delay(1000); //delay selama 1 detik

  //lepas Barang
  //shoulder
  for (pos2; pos2 <= 120; pos2 += 1) { //perulangan untuk menggerakkan servo shoulder ke 120 derajat
    shoulder.write(pos2);
    delay(35);
  }
  //gripper
  for (pos4; pos4 >= 0; pos4 -= 1) { //perulangan untuk menggerakkan servo gripper 0 derajat
    gripper.write(pos4);
    delay(35);
  }
  delay(1000); //delay selama 1 detik
  
  //kembali ke posisi semula
  //shoulder
  for (pos2; pos2 >= 60; pos2 -= 1) { //perulangan untuk menggerakkan servo shoulder ke 60 derajat
    shoulder.write(pos2);
    delay(35);
  }
  //elbow
  for (pos3; pos3 <= 90; pos3 += 1) { //perulangan untuk menggerakkan servo elbow ke 90 derajat
    elbow.write(pos3);
    delay(35);
  }
  //gripper
  for (pos4; pos4 <= 0; pos4 += 1) { //perulangan untuk menggerakkan servo gripper ke 0 derajat
    gripper.write(pos4);
    delay(35);
  }
  //base
  for (pos1; pos1 <= 90; pos1 += 1) { //perulangan untuk menggerakkan servo base ke 90 derajat
    base.write(pos1);
    delay(35);
  }
  delay(5000); //delay selama 1 detik
}