Robobuilder y Arduino

Hace ya la friolera de 5 años me compré un robot humanoide de la marca robobuilder. Después de montar los tres modelos propuestos y juguetear con ellos, poco más pude hacer, ya que al poco nació mi primer hijo y el pobre robot quedó escondido en un cajón.
Desde que empecé con esto de arduino he tenido el gusanillo de querer utilizar los servos con arduino, pero a pesar de tener toda la documentación del protocolo de comunicación que utilizan, no me acababa de decidir porque me daba la impresión de que iba a ser muy complicado.
Estos días, con el gusanillo robótico despierto y a tope, buscando info de otro robot (del que escribiré otro artículo en breve) me topé con una pequeña aplicación para mover un servo SAM-3 de robobuilder con arduino, y aunque no son los que yo tengo (los míos son los wck1111) resulta que utilizan el mismo protocolo aunque requieren de más voltaje.
Después de un par de intentos (por culpa de no leer bien) ya tengo el pequeño ejemplo funcionando :D
Ahora me falta probar dos servos en cadena a ver si funciona, y si funciona, ponerme para hacer una pequeña librería.
Aquí os dejo una fotillo del montaje, que es bastante sencillo.

2014-07-28 16.42.44

A nivel de conexiones sólo hay que conectar tierra, Vc del motor a Vcin de arduino (para que le lleguen los 9V de la pila, y luego el tx al rx de arduino y el rx del motor al tx de arduino.

Y aquí tenéis el código (cambiad el id del motor por el que estéis usando vosotros):
/*
Blink & SAM Control
Turns on an LED and changes a SAM position on for one second, then off for one second, repeatedly.

This example code is in the public domain.
*/

// Pin 13 has an LED connected on most Arduino boards.
// give it a name:
int led = 13;
// for SAM control
int id; // SAM id
int torq; // torque level(0:max, 4:min)
int checksum; // protocol checksum
int tmp1, tmp2; // temporary byte
// the setup routine runs once when you press reset:
void setup() {
// initialize the digital pin as an output.
pinMode(led, OUTPUT);
// start serial port at 115200 bps:
Serial.begin(115200);

id = 16;
torq = 2;
}
// the loop routine runs over and over again forever:
void loop() {
digitalWrite(led, HIGH); // turn the LED on (HIGH is the voltage level)
tmp1 = (torq << 5) | id;
tmp2 = 20; // position1
checksum = (tmp1 ^ tmp2) & 0x7f;
Serial.write(0xff);
Serial.write(tmp1);
Serial.write(tmp2);
Serial.write(checksum);
delay(1000); // wait for a second

digitalWrite(led, LOW); // turn the LED off by making the voltage LOW
tmp1 = (torq << 5) | id;
tmp2 = 120; // position2
checksum = (tmp1 ^ tmp2) & 0x7f;
Serial.write(0xff);
Serial.write(tmp1);
Serial.write(tmp2);
Serial.write(checksum);
delay(1000); // wait for a second
}