/*  tadpole1.ino
    Author: Kyle Kerr
    Date: 3-16-2013
    Purpose: First program to move my Tadpole
*/

// constants based on Mini Driver
const int BATTERYVOLTAGE = A7;
const int LEDPIN = 13;
const int LEFTMOTORSPEED = 9;
const int LEFTMOTORDIRECTION = 7;
const int RIGHTMOTORSPEED = 10;
const int RIGHTMOTORDIRECTION = 8;

//constants based on additional IR sensor
const int IRTRIGGER = 12;
const int LEFTIR = A0;
const int CENTERIR = A1;
const int RIGHTIR = A2;

//constants for motor speed and direction
const int FWD = 1;
const int REV = 0;
const int FAST = 255;
const int MED = 113;
const int SLOW = 40;
//10% change in ratio will be 25
const int RIGHTRATIO = 255;
const int LEFTRATIO = 193;

const int BLANK = 200;
const int TOOCLOSE = 900;

//COUNT controls the number of readings for averaging IR data
const int COUNT = 10;
int leftDistance;
int centerDistance;
int rightDistance;

void setup() {
  Serial.begin(9600);
  pinMode(LEFTMOTORSPEED, OUTPUT);
  pinMode(LEFTMOTORDIRECTION, OUTPUT);
  pinMode(RIGHTMOTORSPEED, OUTPUT);
  pinMode(RIGHTMOTORDIRECTION, OUTPUT);
  pinMode(IRTRIGGER, OUTPUT);
  pinMode(LEDPIN, OUTPUT);
  delay(3000); //wait for 3 secs before getting motivated
}

void loop() {
  if (checkVoltage()) {
    /*
    Forward = robotMove(spd, FWD, spd, FWD, dly);
    Reverse = robotMove(spd, REV, spd, REV, dly);
    Rotate Left = robotMove(spd, REV, spd, FWD, dly);
    Rotate Right = robotMove(spd, FWD, spd, REV, dly);
    Arc Left = robotMove(spd, FWD, faster spd, FWD, dly);
    Stop is not currently required.
    final 0 may be replaced for a delay
    Stop = robotMove(0, FWD, 0, FWD, 0);
    */
    checkDistance();
    if (centerDistance > TOOCLOSE) {
      robotMove(MED, REV, MED, REV, 500);
      robotMove(MED, REV, MED, FWD, 250);
    } else {
      robotMove(FAST, FWD, FAST, FWD, 500);
    }
  }
}

void robotMove(int spdLM, boolean dirLM, int spdRM, boolean dirRM, 
              int dly) {
  analogWrite(LEFTMOTORSPEED, (spdLM * LEFTRATIO / FAST));
  digitalWrite(LEFTMOTORDIRECTION, dirLM);
  analogWrite(RIGHTMOTORSPEED, (spdRM * RIGHTRATIO / FAST));
  digitalWrite(RIGHTMOTORDIRECTION, dirRM);
  delay(dly);
}

boolean checkVoltage() {
  boolean flag = 0;
  //512 is said to be 5v. I want to stop before I get there.
  if (analogRead(BATTERYVOLTAGE) > 513) {
    flag = 1;
  } else {
    robotMove(0, FWD, 0, FWD, 0);
    while(1) {
      blinkLed3(200);
      blinkLed3(500);
      blinkLed3(200);
      delay(500);
    }
  }
  return flag;
}

void blinkLed3(int dly) {
  boolean ledState = LOW;
  for(int i=0; i <= 2; ++i) {
    ledState = !ledState;
    digitalWrite(LEDPIN, ledState);
    delay(dly);
    ledState = !ledState;
    digitalWrite(LEDPIN, ledState);
    delay(BLANK);
  }
}

void checkDistance() {
  int l, r, c;
  int i = 0;
  digitalWrite(IRTRIGGER, HIGH);
  while (i < COUNT) {
    l = analogRead(LEFTIR);
    c = analogRead(CENTERIR);
    r = analogRead(RIGHTIR);
    leftDistance += l;
    centerDistance += c;
    rightDistance += r;
    ++i;
  }
  leftDistance /= COUNT;
  centerDistance /= COUNT;
  rightDistance /= COUNT;
  digitalWrite(IRTRIGGER, LOW);
//  Serial.print(leftDistance);
//  Serial.print("\t");
//  Serial.print(centerDistance);
//  Serial.print("\t");
//  Serial.println(rightDistance);
}