Control de encoder Pololu usando arduino y gambas

Qué tal,

Pues bien, estoy realizando un robot, pero como el trabajo no me deja mucho tiempo libre voy de poco en poco.

Para la parte de los motores tengo pensado usar Micro Metal Gearmotors (aunque no los he comprado en Pololu, hay opciones más baratas en ebay), Encoders opticos de pololu y una shield para arduino, igualmente de Pololu, la DRV8835. Y obviamente un arduino (he hecho las pruebas en un arduino Duemilanove pero el código debe ser compatible también con arduino UNO).

El encoder de pololu la verdad que me ha agradado mucho, se comporta muy bien, aunque en la instalación de las librerías me he liado un poco.
La librería usada para el encoder fue libpololu-arduino-150218.zip la cual es compatible con el entorno arduino 1.6.x.

Y la shield DRV885 utiliza la librería drv8835-motor-shield.

Para la comunicación entre mi aplicación PC en gambas y arduino hago uso del puerto serie.

Así que el código en la parte de arduino es:

#include <DRV8835MotorShield.h>
#include <PololuWheelEncoders.h>

DRV8835MotorShield motors;
PololuWheelEncoders encoder;

int mcount = 0;
int mcount_limit = 0;
byte byteReadH;
byte byteReadL;

void setup() {
  encoder.init(2,3,4,5);
  pinMode(2, INPUT);
  pinMode(3, INPUT);
  pinMode(4, INPUT);
  pinMode(5, INPUT);
  Serial.begin(9600);
  delay(500);
}

void loop() {

  if (Serial.available() >= 5) {
    byteReadH = Serial.read();
    Serial.write(byteReadH);
    if (byteReadH == '$' ) {
      byteReadH = Serial.read();
      byteReadL = Serial.read();
      //byteReadH = 0x00;
      //byteReadL = 0x32;
      Serial.write(byteReadH);
      Serial.print(", ");
      Serial.write(byteReadL);
      Serial.print(", ");
      Serial.write(byteReadH << 8 | byteReadL);
      Serial.println();
      motors.setM1Speed(byteReadH << 8 | byteReadL);
      byteReadH = Serial.read();
      byteReadL = Serial.read();
      mcount_limit = byteReadH << 8 | byteReadL;
      Serial.flush();
    }
   else
    {
    Serial.flush();
    }
  }
  mcount =encoder.getCountsM1();
  if (mcount >= mcount_limit) {
    motors.setM1Speed(0);
    encoder.getCountsAndResetM1();
  }
}  

Y para la parte de gamba, me basé en el proyecto de ejemplo del puerto serie.

Los archivos con el código usado son:
SerialPort-0.1.tar.gz
stop_encoder_serial.ino

Y a continuación un vídeo mostrando el uso del encoder de Pololu: