switch para hacer girar servos

[Superboy]

Hola buena noche, apenas empiezo a programar y me gustaria saber si alguien puede ayudarme a saber como puedo definir en el codigo que tengo las variables para manejar varios servos, este es un switch en el que yo toco un boton y el servo gira a 180° y si vuelvo a tocar el boton regresa a 0°
el codigo es este:

Código: C++

1. val1=getc();
2.  
3.    
4.     if ((val1=='1' && val2=='2'))
5.     {
6.    
7.     state=1-state;
8.     delay_us(20);
9.    
10.     }
11.     val2 = val1;
12.        
13. if (state == 1)
14. {
15.      output_high(servo);
16.      delay_us(2500); /// 180
17.      output_low(servo);
18.      delay_ms(20);
19.      output_high(LED1);  
20. }
21.  
22. else
23. {
24.      output_high(servo);
25.      delay_us(400); /// 0
26.      output_low(servo);
27.      delay_ms(20);
28.      output_low(LED1);  
29.  
30. }

estoy enviando por programación serial los caracteres '1' y '2' de val1 y val2 pero cuando quiero manejar otro servo no funciona el programa y ya intente definiendo un state1, definiendo un val3 y un val4 para otro servo, pero no funciona, espero me puedan ayudar y me haya explicado bien.