#include <Servo.h>

Servo vrataServo; //door servo
Servo rukaServo; //hand servo

const int vrataPin = 10; //pin for door
const int rukaPin = 11; //pin for hand
int prekidacPin = 2; //switch on/off pin

int pozicijaRuka = 0; //position hand
int pozicijaVrata = 0; //position door
//I will change this values later when my servo comes.

int pozicija = 0; // position of hand and door

void setup()
{
  Serial.begin(9600);
  pinMode(prekidacPin, INPUT); //switch on/off is set to INPUT mode
  //Attaching door and hand servo to pin 10, 11 and write position of them
  rukaServo.attach(rukaPin);
  vrataServo.attach(vrataPin);
  rukaServo.write(pozicijaRuka);
  vrataServo.write(pozicijaVrata);
}

void loop()
{
  if(digitalRead(prekidacPin) == HIGH) //if the switch is on
  {
      pokreniServo(); //start moving hand and door
  }
}

void pokreniServo()
{
  //open door
  for(pozicija = 80; pozicija < 155; pozicija += 3)
  {
    vrataServo.write(pozicija);
    delay(15);
  }
  //open hand
  for(pozicija = 0; pozicija < 129; pozicija += 4)
  {
    rukaServo.write(pozicija);
    delay(15);
  }
  //Hide hand
  for(pozicija = 129; pozicija >= 0; pozicija -= 4)
  {
    rukaServo.write(pozicija);
    delay(15);
  }
  //Hide door
  for(pozicija = 155; pozicija >= 80; pozicija -= 3)
  {
    vrataServo.write(pozicija);
    delay(15);
  }
}