#include <Servo.h>
char bufor[11];
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
void setup()
{
servo1.attach(11);
servo2.attach(10);
servo3.attach(9);
servo4.attach(6);
servo5.attach(5);
Serial.begin(9600);
while(Serial.available())
Serial.read();
servo1.write(90); 
servo2.write(90);
servo3.write(90);
servo4.write(90);
servo5.write(90);
Serial.println("A - serwo pod pinem 5");
Serial.println("B - serwo pod pinem 6");
Serial.println("C - serwo pod pinem 9");
Serial.println("D - serwo pod pinem 10");
Serial.println("E - serwo pod pinem 11");
Serial.println("Wpisz litere i kat obrotu, np: A180");
}



void loop()
{
if (Serial.available() > 0) {
int index=0;
delay(100);
int liczbaznakow = Serial.available(); 
if (liczbaznakow>10) {
liczbaznakow=10;
}
while (liczbaznakow--) {
bufor[index++] = Serial.read();
}
bufor[index]='\0';
ustawServo(bufor);
}
}

void ustawServo(char* data) {
if ((data[0] == 'A') || (data[0] == 'a')) {
int p_wartosc = strtol(data+1, NULL, 10);
p_wartosc = constrain(p_wartosc,0,180);
servo1.write(p_wartosc);
Serial.print("Servo1 ustawiono na: ");
Serial.println(p_wartosc);
}
if ((data[0] == 'B') || (data[0] == 'b')) {
int d_wartosc = strtol(data+1, NULL, 10);
d_wartosc = constrain(d_wartosc,0,255);
servo2.write(d_wartosc);
Serial.print("Servo2 ustawiono na: ");
Serial.println(d_wartosc);
}
if ((data[0] == 'C') || (data[0] == 'c')) {
int t_wartosc = strtol(data+1, NULL, 10);
t_wartosc = constrain(t_wartosc,0,255);
servo3.write(t_wartosc);
Serial.print("Servo3 ustawiono na: ");
Serial.println(t_wartosc);
}
if ((data[0] == 'D') || (data[0] == 'd')) {
int c_wartosc = strtol(data+1, NULL, 10);
c_wartosc = constrain(c_wartosc,0,255);
servo4.write(c_wartosc);
Serial.print("Servo4 ustawiono na: ");
Serial.println(c_wartosc);
}
if ((data[0] == 'E') || (data[0] == 'e')) {
int pi_wartosc = strtol(data+1, NULL, 10);
pi_wartosc = constrain(pi_wartosc,0,255);
servo5.write(pi_wartosc);
Serial.print("Servo5 ustawiono na: ");
Serial.println(pi_wartosc);
}
}