/*
Testing Sharp IR sensor on Arduino Diecimila
Posted to letsmakerobots.com on May 31, 2008
Version 2.01

Equipment used and pin outs:
*  Arduino Diecimila
*  Sharp GP2Y0A21YK0F Distance Sensor
      Pin 1 => Analog 2
      Pin 2 => Gnd
      Pin 3 => +5V
*  1K 5% resistor (Brown Black Red Gold)
*  T-1 3/4 5mm Full Color LED (Radio Shack part #276-028)
      Pin 1 => Digital 9
      Pin 2 => 1K resistor => +5V
      Pin 3 => Digital 10
      Pin 4 => Digital 11

*/


//==========================================================================================
// Some variables we will need
//==========================================================================================

//==========================================================================================
// avgFactor defines how many readings to average, more readings is slower but more accurate
//==========================================================================================
int avgFactor = 5;
int runningTotal;
int distRead;
int distAvg;
int i;

//==========================================================================================
// Define I/O pins:
//==========================================================================================
int ledR = 9;       // Red channel
int ledB = 10;      // Blue channel
int ledG = 11;      // Green channel
int sharpIR = 2;    // Sharp IRED output on Analog 2



//==========================================================================================
// Setup:
//==========================================================================================
void setup()
{
  Serial.begin(9600);
  testLED();
}



//==========================================================================================
// Main loop:
//==========================================================================================
void loop()
{
runningTotal = 0;
distRead = 0;
distAvg = 0;  
  getDist();
  colorLEDs(distAvg);
}



//==========================================================================================
// Funtions:
//==========================================================================================


//==========================================================================================
// Get distance from Sharp IRED unit:
//==========================================================================================
int getDist()
{
  for (i=0; i < avgFactor; i++)
  {
    distRead = analogRead(sharpIR);
    runningTotal += distRead;
    // Diagnostic ouptut:
    //Serial.print("i = ");
    //Serial.println(i, DEC);
    //Serial.print("runningTotal = ");
    //Serial.println(runningTotal, DEC);
  }
  distAvg = (runningTotal / avgFactor);
  // Diagnostic ouptut:
  //Serial.print("distAvg = ");
  //Serial.println(distAvg, DEC);
  return distAvg;
}

//==========================================================================================
// Function to change LED color based on distance:
//==========================================================================================
int colorLEDs(int)
{
  if (distAvg >= 500)  //Turn on LED red if > 500
  {
  redLED();
  }
  else if ((distAvg >= 200) && (distAvg < 500))  //Turn on LED yellow-ish if 200 < dist > 500
  {
  yellowLED();
  }
  else if (distAvg < 200)  //Turn on LED green if dist < 199
  {
  greenLED();
  }
}

//==========================================================================================
// Just to test LED, run through each color & turn off:
//==========================================================================================
int testLED()
{
  redLED();
  delay(500);
  yellowLED();
  delay(500); 
  greenLED();
  delay(500);
  offLED();
}

//==========================================================================================
// LED Colors functions
//==========================================================================================

//==========================================================================================
// Light up LED Red 
//==========================================================================================
int redLED() {
  analogWrite(ledR, 0);
  analogWrite(ledB, 255);
  analogWrite(ledG, 255);
}

//==========================================================================================
// Light up LED Yellow
//==========================================================================================
int yellowLED() {
  analogWrite(ledR, 70);
  analogWrite(ledB, 240);
  analogWrite(ledG, 10);
}

//==========================================================================================
// Light up LED Green
//==========================================================================================
int greenLED() {
  analogWrite(ledR, 255);
  analogWrite(ledB, 255);
  analogWrite(ledG, 0);
}

//==========================================================================================
// Turn off LED
//==========================================================================================
int offLED() {
  analogWrite(ledR, 255);
  analogWrite(ledG, 255);
  analogWrite(ledB, 255);
}