#include <Servo.h>

#define cervene 0
#define zelene 1
#define modre 2

Servo servo1;
Servo servo2;
Servo servo3;

byte servo = 0;
unsigned long cas = 0;
bool zmena = false;


void setup() 
{
  DDRC &= ~(1<<cervene);
  DDRC &= ~(1<<zelene);
  DDRC &= ~(1<<modre);

  PORTC |= 1<<cervene;
  PORTC |= 1<<zelene;
  PORTC |= 1<<modre;

  SREG |= 1<<7;
  PCICR |= 1<<PCIE1;
  PCMSK1 |= 0b00000111;
  
  servo1.attach(11);
  servo2.attach(10);
  servo3.attach(9);

  servo1.write(90);
  servo2.write(90);
  servo3.write(90);
}

void loop() 
{
  if(zmena)
  {
    zmena = false;
    
    if(servo == 1)
      servo1.write(0);
    
    if(servo == 2)
      servo2.write(0);

    if(servo == 3)
      servo3.write(0);

    if(servo == 0)
    {
      servo1.write(90);
      servo2.write(90);
      servo3.write(90);
    }
    cas = millis();  
  }

  if((millis() > (cas + 2000)) && (!zmena))
  {
    servo1.write(90);
    servo2.write(90);
    servo3.write(90);
    PCICR |= 1<<PCIE1;
  }
}

ISR(PCINT1_vect)
{
  zmena = true;
  PCICR &= ~(1<<PCIE1);
  if(!(PINC & 1<<cervene))
    servo = 1;
  if(!(PINC & 1<<zelene))
    servo = 2;
  if(!(PINC & 1<<modre))
    servo = 3;
}