#include <Servo.h>

//0 - hore
//90 - dopredu
//180 - dolu

//serva:
Servo base;
Servo middle_down;
Servo middle_up;
Servo claw;

int base_val = 180;
int middle_down_val = 30;
int middle_up_val = 0;
int claw_val = 80;

//button
const int buttonPin = 7;
int buttonState = 0;

void setup() {
  Serial.begin(9600);
  base.attach(2);
  middle_down.attach(3);
  middle_up.attach(4);
  claw.attach(5);
  zeroPosition();

  pinMode(buttonPin, INPUT);
}

void loop() {

  buttonState = digitalRead(buttonPin);
      
  if (buttonState == HIGH) 
  {
    grab();
    drop();
  } 
  
  else 
  {
    zeroPosition();
  }
}

void zeroPosition()
{
  base.write(base_val);
  middle_down.write(middle_down_val);
  middle_up.write(middle_up_val);
  claw.write(claw_val);
}

void grab()
{
  for(int i = 180; i >= 45; i--)
  {
    base.write(i);
    delay(20);
  }
 
  for(int i = 30; i <= 100; i++)
  {
    middle_up.write(i);
    delay(20);
  }  

  for(int i = 0; i <= 90; i++)
  {
    middle_down.write(i);
    delay(20);
  }

  for(int i = 80; i <= 110; i++)
  {
    claw.write(i);
    delay(20);
  }

}

void drop()
{
  for(int i = 90; i >= 45; i--)
  {
    middle_down.write(i);
    delay(20);
  }

  for(int i = 100; i >= 30; i--)
  {
    middle_up.write(i);
    delay(20);
  } 

  for(int i = 45; i <= 130; i++)
  {
    base.write(i);
    delay(20);
  }

  for(int i = 110; i >= 80; i--)
  {
    claw.write(i);
    delay(20);
  }
}