move_a_meter.ino

// This program shows how to read the encoders on the Romi 32U4.
// The encoders can tell you how far, and in which direction each
// motor has turned.
//
// You can press button A on the Romi to drive both motors
// forward at full speed.  You can press button C to drive both
// motors in reverse at full speed.
//
// Encoder counts are printed to the LCD and to the serial
// monitor.
//
// On the LCD, the top line shows the counts from the left
// encoder, and the bottom line shows the counts from the right
// encoder.  Encoder errors should not happen, but if one does
// happen then the buzzer will beep and an exclamation mark will
// appear temporarily on the LCD.
//
// In the serial monitor, the first and second numbers represent
// counts from the left and right encoders, respectively.  The
// third and fourth numbers represent errors from the left and
// right encoders, respectively.

#include <Romi32U4.h>

Romi32U4Encoders encoders;
Romi32U4LCD lcd;
Romi32U4Buzzer buzzer;
Romi32U4Motors motors;
Romi32U4ButtonA buttonA;
Romi32U4ButtonC buttonC;

const char encoderErrorLeft[] PROGMEM = "!<c2";
const char encoderErrorRight[] PROGMEM = "!<e2";

char report[80];

int countsPerRev = 1440;
int wheelDiameter = 70; //mm
double wheelCircumference = wheelDiameter*3.14159;
double mmPerCount = wheelCircumference / countsPerRev;
int countsPerMeter = 1000 / mmPerCount;
int currentCountLeft = 0;
int currentCountRight = 0;
int offsetL = 0;
int offsetR = 1;

void setup()
{
  Serial.begin(9600);
  Serial.println(countsPerMeter);
  buttonA.waitForButton();
  motors.setSpeeds(50+offsetL,50+offsetR);
}

void loop()
{
  //Move a meter
  currentCountLeft = encoders.getCountsLeft();
  currentCountRight = encoders.getCountsRight();
  if( currentCountLeft >= countsPerMeter && currentCountRight >= countsPerMeter){
    motors.setSpeeds(0,0);
  }
}