/** RoombaPhotovore: Turns your Roomba into a light follower by controlling it via IR.
 *  Uses a 38 KHz carrier generated by directly setting timer 2's registers.
 *  Author: Tom Lauwers, tlauwers@birdbraintechnologies.com
 *  Date: 1/3/2011
 *
 *  Modified: 01/24/2012 by Max Eliaser of O'Reilly Media, Inc.
 *  Capitalize preprocessor macro names.
 *  Add LEFT_LIGHT_PIN, RIGHT_LIGHT_PIN, LIGHT_THRESHOLD, LIGHT_DIFF_THRESHOLD, and IR_OUTPUT_PIN
 *  macros where previously there had been hardcoded numbers.
 */

// The length of the raw IR signals
#define SIGNAL_LENGTH 15
// The pins for the LED and photoresistors
#define IR_OUTPUT_PIN 11
#define LEFT_LIGHT_PIN 0
#define RIGHT_LIGHT_PIN 1
// Twiddle this to adjust for the peculiarities of your photoresistors and the ambient light levels
#define LIGHT_THRESHOLD 800 // was originally 100; I changed it to 800 to compensate for a bright room
// Twiddle this to adjust how different the photoresistors should be to trigger a turn
#define LIGHT_DIFF_THRESHOLD 100

// Arrays that contain the raw IR signal times in microseconds
// Captured directly from IRAnalyzer
unsigned int goForward[SIGNAL_LENGTH] = {3028,956,996,2948,956,2948,960,2948,960,2940,964,2948,2896,984,996};
unsigned int turnLeft[SIGNAL_LENGTH] = {2992,1008,968,2964,944,2964,944,2964,940,2972,936,2976,932,2976,2872};
unsigned int turnRight[SIGNAL_LENGTH] = {2988,1012,968,2964,936,2976,940,2968,932,2976,932,2976,2872,1012,2904};

// For storing the left and right light sensor values
int leftLight;
int rightLight;

void setup()
{
  // start serial port at 57600 bps:
  Serial.begin(57600);
  // PWM Magic - we directly set atmega registers to 50% duty cycle,
  // variable frequency mode. Currently set to 38 KHz.
  // Thanks http://arduino.cc/en/Tutorial/SecretsOfArduinoPWM
  TCCR2A = _BV(WGM21) | _BV(WGM20);
  //set prescalar to 16MHz clock for now
  TCCR2B = _BV(WGM22) | _BV(CS20);
  // To get a 38KHz wave, we need OCR2A = (16000000/76000)-1 = 209
  OCR2A = 209;
  // Set the pin the IR LED is on to output
  pinMode(IR_OUTPUT_PIN, OUTPUT);
}

void loop()
{
  // Read in light sensors - left on pin 0, right on pin 1
  leftLight = analogRead(LEFT_LIGHT_PIN);
  rightLight = analogRead(RIGHT_LIGHT_PIN);

  // Print the values for debugging purposes
  Serial.print("Left: ");
  Serial.print(leftLight);
  Serial.print(" Right: ");
  Serial.print(rightLight);
  Serial.print("\n");

  // If both sensors see very little light, stop!
  if((leftLight < LIGHT_THRESHOLD) && (rightLight < LIGHT_THRESHOLD)) {
    // do nothing, effectively stops Roomba
  }
  // If left is a lot less than right, turn right to balance it out
  else if(leftLight < (rightLight - LIGHT_DIFF_THRESHOLD)) {
    playSignal(turnRight);
  }
  // If right is less than left, turn left
  else if(rightLight < (leftLight - LIGHT_DIFF_THRESHOLD)) {
    playSignal(turnLeft);
  }
  // else, go straight towards the light
  else {
    playSignal(goForward);
  }
  // Roomba needs a call to playSignal to repeat every 20 milliseconds
  delay(20);
}

// Plays raw signal
void playSignal(unsigned int signal[])
{
  // Traverse the raw IR signal array, turning the LED on or off by the amount of time
  // specified by the array
  for(int i = 0; i < SIGNAL_LENGTH; i++)
  {
    // even elements of the array = IR LED is on
    if(i%2 == 0) {
      TCCR2A |=  _BV(COM2A0); // turns on the IR LED to 50% duty cycle
      delayMicroseconds(signal[i]);
    }
    // IR LED is off for odd elements of the array
    else {
      TCCR2A &= (0xFF - _BV(COM2A0)); // disconnect pin from signal
      digitalWrite(IR_OUTPUT_PIN,LOW); // turn pin low in case it's high
      delayMicroseconds(signal[i]);
    }
  }
  // Turn off signal one last time
  TCCR2A &= (0xFF - _BV(COM2A0)); // disconnect pin from signal
  digitalWrite(IR_OUTPUT_PIN,LOW); // turn pin low in case it's high
}