Robot Kuba

#define LED_L 12 // pin
#define LED_P 13 // pin
#define hlava 10 // PWM pin
#define rukaL 11 // PWM pin
#define rukaP 9 // PWM pin
#define kratsi 15 // ms
#define delsi 25 // ms
// po fci attach() bude vsude 90 stupnu
byte pozice[] = {90,90,90};
byte nova; // nova pozice
Servo servoHlava;
Servo servoRukaL;
Servo servoRukaP;

void serva(byte poz){
nova = random(1,180);
if (pozice[poz]<nova)
{
for (byte stara=pozice[poz];stara<=nova;stara++)
{
switch (poz)
{
case 0:
servoHlava.write(stara);
break;
case 1:
servoRukaL.write(stara);
break;
case 2:
servoRukaP.write(stara);
break;
default:
delay(kratsi);
break;
}
delay(kratsi);
}
}
else if (pozice[poz]>nova)
{
for (byte stara=pozice[poz];stara>=nova;stara--)
{
switch (poz){
case 0:
servoHlava.write(stara);
break;
case 1:
servoRukaL.write(stara);
break;
case 2:
servoRukaP.write(stara);
break;
default:
delay(kratsi);
break;
}
delay(kratsi);
}
}
else
{
delay(delsi);
}
pozice[poz]=nova;
}

void setup()
{
servoHlava.attach(hlava);
delay(350);
servoRukaL.attach(rukaL);
delay(350);
servoRukaP.attach(rukaP);
delay(350);
randomSeed(analogRead(A4)); // el.mag. sum
pinMode(LED_L,OUTPUT);
pinMode(LED_P,OUTPUT);
}