Restarting SLAM project

I've begun the steps necessary to restart my original SLAM project I posted some time ago. I always wanted to not only complete the project but keep up this blog to document my progress and hopefully help others in their robot endeavors.

Added stepper motors.

I will be using the same platform as the original post, however I have changed over to using stepper drive motors instead of the cheap DC gear motors I was using. The motors were retrofitted onto the robot chassis with a few new holes drilled and taking a hot iron to the wheels to make room for the stepper motor shafts. I used AutoCAD to make a 1:1 layout of the motor mount and the location of the new mounting holes for steppers. 

I've run some tests on controlling the stepper motors with an Arduino Micro and two A4988 Stepper controller breakout boards from Pololu. They are in 16th step mode to reduce the speed of the robot, this is accomplished by tying the 3 MS# pins to +5V, see the datasheet for details.

Test schematic.

Using the above setup and the following code, I was able to make the robot move forward and back for a full rotation continuously.

// Define pinout
int leftStepPin = 7;
int leftDirPin = 8;
int rightStepPin = 9;
int rightDirPin = 10;
int curDir = 0;

void setup() {
  // Start serial coms
  Serial.begin(9600);

  // Configure right motor pins 
  pinMode(rightDirPin, OUTPUT);
  pinMode(rightStepPin, OUTPUT);
  digitalWrite(rightDirPin, HIGH);

  // Configure left motor pins
  pinMode(leftDirPin, OUTPUT);
  pinMode(leftStepPin, OUTPUT);
  digitalWrite(leftDirPin, HIGH);
}

// Loop function
void loop() {

  // Complete single rotation of motor
  for( int i = 0; i < 3200; i++) {
    digitalWrite(rightStepPin,LOW);
    digitalWrite(leftStepPin,LOW);
    delay(1);
    digitalWrite(rightStepPin,HIGH);
    digitalWrite(leftStepPin,HIGH);
    delay(1);
  }

  // Swap direction for each motor
  if( curDir == 1 ) {
    digitalWrite(rightDirPin, LOW);
    digitalWrite(leftDirPin, LOW);
    curDir = 0;
  }
  else {
    digitalWrite(rightDirPin, HIGH);
    digitalWrite(leftDirPin, HIGH);
    curDir = 1;
  }
}