Control Odometria Irobot Create

[Ricardux]

buenas estoy intentando crear el codigo en mikroCfordspic para hacer el control de la posicion del irobotcreate.
Es decir lo que realmente pretendo es que el robot me este enviando sus datos de posicion (x,y y theta).
He encontrado un monton de info sobre esto pero nunca un script en C, donde pueda implementarlo.
Cuento con la distancia recorrida y el angulo que me dan los sensores del create. Y segun entiendo es relativamente sencillo, ya que es el tipico robot.
Agradezco cualquier sugerencia de por donde empezar ya que me siento algo perdido y no se por donde hecharle el diente como decimos aca. salu2

[Ricardux]

Bueno investigando y aclarandome un poco mas se que mi robot es de traccion diferencial, por lo cual puedo usar unas ecuaciones sencillas para saber su ubicacion.
Como son las siguientes:

desplazamiento = (desplazoR + despalzoIzq)/2
theta= {(desplazoR-desplazoizq)/b} + thetainicial
x=desplazamiento.cos(theta) +xinicial
y=desplazamiento.sen(theta)+yinicial

siendo b la distancia de los ejes.

como el create me da el desplazamiento y el angulo tendria resuelto la ubicacion.

pero como mi tarea consiste en que un robot siga el otro, es decir uno le envie al otro sus coordenadas y este a su vez lo siga.

tendria que pasar las coordenadas a velocidades que son las que recibe el create para moverse, Vr y Vl, alguna idea?