Lego Stepperbot

#include <IRremote.h>

IRrecv irrecv(A0);
decode_results results;

//delay between subsequent IR reads, to avoid getting muliple presses
#define IR_DELAY 250  

//last ir decoded, micros
long lastIR = 0; 

#define MOVE_STEPS 2000
#define TURN_STEPS 3000
#define STEP_DELAY 1000

//I can never make up my mind on the step sequence. Both works obviously, need to measure them properly some day to see what really works best.
byte stepSequence[8] = {B1100, B1100, B0110, B0110, B0011, B0011, B1001, B1001};  
//byte stepSequence[8] = {B1000, B1100, B0100, B0110, B0010, B0011, B0001, B1001};

byte leftPins[4]  = {2, 3, 4, 5};
byte rightPins[4] = {6, 7, 8, 9};

//variables keeping track of current position of steppers
long currentLeft=0, currentRight=0;

//steps left to step, positive or negative
long stepLeft=0, stepRight=0;

//when was the last step done, micros
long lastStepChange = 0; 

//this is where the piezo beeper sits
#define TONE_PIN 11

void setup() {
    for(int pin=0 ; pin<4 ; pin++) {
      pinMode(leftPins[pin], OUTPUT);
      pinMode(rightPins[pin], OUTPUT);
    } 

  irrecv.enableIRIn(); 
  
  Serial.begin(9600);
  Serial.println("Hello Serial!");

  pinMode(TONE_PIN, OUTPUT);  
}

/*
 * The stepping function. Will step _one_ step in the direction specified by stepsToGo and update variables accordingly
 */
void step(long* stepsToGo, long *currPos, byte *pins) {
  
  if(*stepsToGo != 0) {
      if(*stepsToGo > 0) {
         (*stepsToGo)--;
         (*currPos)++; 
      }
      else {
         (*stepsToGo)++;
         (*currPos)--;                
      }
  
      byte mask = stepSequence[*currPos & 0x7];
      for(int bit=0 ; bit<4 ; bit++) {        
        digitalWrite(pins[bit], mask & (B1000 >> bit));
      }
      lastStepChange = micros();     
  }
  else {    
      //switch off all pins to save power when idle
      for(int bit=0 ; bit<4 ; bit++) {        
        digitalWrite(pins[bit], 0);
      }
  }
}

//some nice frequencies for sound
int tones[] = {262, 294, 330, 349};

//moves are stored in the "moves" queue
typedef enum {FWD, BACK, LEFT, RIGHT} move;
move moves[100];
int nMoves = 0, currMove = 0;

bool run = false;

/*
 * Simplistic tone generation function. Have this to avoid using timers that conflict with other libraries. 
 * f is frequency in hz
 * d is duration in milliseconds
 */
void myTone(int f, int d) {
  bool state = 0;
  unsigned long now = micros();
  unsigned long stopAt = now + d*(long)1000;
  unsigned long dt = 1000000 / (2*(long)f);  //dt is for the double frequency  
  unsigned long nextChange = now+dt;

  while (stopAt > micros()) {
    if(micros() > nextChange) {
      digitalWrite(TONE_PIN, state ? HIGH : LOW);
      nextChange += dt;
      state = !state;
    }    
  }
  digitalWrite(TONE_PIN, LOW); //end on a low (assuming beeper is connected to GND on the other end)
}

/*
 * The allmighty loop function.
 * Does three things:
 * - step the steppers (one step at a time).
 * - update how steppers should run according to move queue.
 * - read IR to program the move queue and start/stop things.
 */
void loop() {
   if(micros()-lastStepChange > STEP_DELAY) {
      step(&stepLeft, &currentLeft, leftPins);
      step(&stepRight, &currentRight, rightPins);
   }     

   if(run) {
      if(currMove == nMoves) {
        nMoves = currMove = 0;         
        run = false;        
      }
      else if(currMove < nMoves && stepLeft == 0 && stepRight == 0) {
        switch(moves[currMove++]) {
          case FWD:
             Serial.println("FWD");
             stepLeft += MOVE_STEPS;
             stepRight += MOVE_STEPS;         
             break;         
           case BACK:
             Serial.println("BACK");
             stepLeft -= MOVE_STEPS;
             stepRight -= MOVE_STEPS;         
             break;         
           case LEFT:
             Serial.println("LEFT");
             stepLeft += TURN_STEPS;
             stepRight -= TURN_STEPS;         
             break;         
           case RIGHT:
             Serial.println("RIGHT");
             stepLeft -= TURN_STEPS;
             stepRight += TURN_STEPS;         
             break;         
        }
      }      
   }
   
  if (micros()-lastIR > IR_DELAY && irrecv.decode(&results)) {         
    switch(results.value) {
       case 0x1FE10EF: moves[nMoves++] = FWD; myTone(tones[FWD], 100); break; 
       case 0x1FEB04F: moves[nMoves++] = BACK; myTone(tones[BACK], 100); break; 
       case 0x1FE50AF: moves[nMoves++] = LEFT; myTone(tones[LEFT], 100); break; 
       case 0x1FEF807: moves[nMoves++] = RIGHT; myTone(tones[RIGHT], 100); break; 
       case 0x1FE7887: //start running! 
            run=true;
            Serial.print("Start! moves="); 
            Serial.println(nMoves);
            //play through the sounds of the queue, just because blip blop...
            for(int i=0; i<nMoves; i++) {
              myTone(tones[moves[i]], 100);
              delay(50);
            }
            break; 
       case 0x1FE48B7: //Stop all and clear queue
            nMoves=currMove=0;
            stepLeft=stepRight=0;
            run=false;
            break; 
       default:
         //this printout will give you the hex-values for the keys on your remote, just update the cases above with your values
         Serial.println(results.value, HEX);
     }
    irrecv.resume(); 
    lastIR = micros();     
  }

}