My Tracking Robo

robot.cpp

int sensorRight = 0;
int sensorMid = 1;
int sensorLeft = 2;

int sensorLeftBound = 500;
int sensorRightBound = 500;
int sensorMidBound = 500;

int forwardCount = 0;

int ledLeft = 12;
int ledRight = 11;

int sound = 4;

int motorLeft[2] = {3, 2};
int motorRight[2] = {10, 5};


const int WHITE = 111;
const int BLACK = 112;

const int FORWARD = 123;
const int BACKWARD = 124;
const int HALT = 125;

const int LEFT = 139;
const int RIGHT = 140;
const int STRAIGHT = 141;

int lastTurn = STRAIGHT;
int whiteCount = 0;

int senseLeft, senseRight, senseMid; 

void beep(int tn){
  //byte names[] = {'c', 'd', 'e', 'f', 'g', 'a', 'b', 'C'};  
  int tones[] = {1915, 1700, 1519, 1432, 1275, 1136, 1014, 956};
  analogWrite(sound, 0);
  int i;
  for(i = 0;i < 100;i++){
    analogWrite(sound,500);
    delayMicroseconds(tones[tn]);
    analogWrite(sound, 0);
    delayMicroseconds(tones[tn]);
  }
}

void setup() {
  // declare the ledPin as an OUTPUT:
  Serial.begin(19200);
  pinMode(motorLeft[0], OUTPUT);
  pinMode(motorLeft[1], OUTPUT);
  pinMode(motorRight[0], OUTPUT);
  pinMode(motorRight[1], OUTPUT);  
  pinMode(sound, OUTPUT);
  
  int sensorIn, maxSensorIn = 0;
  for(int i = 0;i < 100;i++) {
    sensorIn = analogRead(sensorLeft);
    if(sensorIn > maxSensorIn) {
      maxSensorIn = sensorIn;
    }
  }
  sensorLeftBound = maxSensorIn + 80;
  maxSensorIn = 0;
  for(int i = 0;i < 100;i++) {
    sensorIn = analogRead(sensorRight);
    if(sensorIn > maxSensorIn) {
      maxSensorIn = sensorIn;
    }
  }
  sensorRightBound = maxSensorIn + 80;
  maxSensorIn = 0;
  for(int i = 0;i < 100;i++) {
    sensorIn = analogRead(sensorMid);
    if(sensorIn > maxSensorIn) {
      maxSensorIn = sensorIn;
    }
  }
  sensorMidBound = maxSensorIn + 80;
  beep(0);
  delay(1000);
  beep(0);
  delay(1000);
  beep(1);
  Serial.println("Sensor LEFT = ");
  Serial.println(sensorLeftBound);
  Serial.println("Sensor Right = ");
  Serial.println(sensorRightBound);
  Serial.println("Sensor Mid = ");
  Serial.println(sensorMidBound);
}

void drive(int *motor, int direct){
  if(direct == FORWARD) {
    digitalWrite(motor[0], HIGH);
    digitalWrite(motor[1], LOW);
  }
  if(direct == BACKWARD) {
    digitalWrite(motor[0], LOW);
    digitalWrite(motor[1], HIGH);
  }
  if(direct == HALT) {
    digitalWrite(motor[0], LOW);
    digitalWrite(motor[1], LOW);
  }
}

int sense(int pin, int bound) {
  int sensorIn = 0;
  int maxSensorIn = 0;
  int minSensorIn = 1024;
  for(int i = 0;i < 5;i++) {
    sensorIn = analogRead(pin);
    if(sensorIn > maxSensorIn) {
      maxSensorIn = sensorIn;
    }
  }
  Serial.print("SENSING : ");
  Serial.println(maxSensorIn);
  return (maxSensorIn < bound) ? WHITE : BLACK;
}

void turnRight() {
  Serial.println("Right Forward");
  lastTurn = RIGHT;
  drive(motorLeft, FORWARD);
}

void turnLeft() {
  Serial.println("Left Forward");
  lastTurn = LEFT;
  drive(motorRight, FORWARD);
}

void goStraight() {
  Serial.println("Forward");
  lastTurn = STRAIGHT;
  drive(motorRight, FORWARD);
  drive(motorLeft, FORWARD);
}

void loop() {
  Serial.println("[WTF]");
  Serial.println(":: LEFT : ");
  senseLeft = sense(sensorLeft, sensorLeftBound);
  Serial.println(":: Right : ");
  senseRight = sense(sensorRight, sensorRightBound);
  Serial.println(":: MidM : ");
  senseMid = sense(sensorMid, sensorMidBound);

  forwardCount = 0;
  drive(motorRight, HALT);
  drive(motorLeft, HALT);

  if (senseLeft == WHITE && senseRight == WHITE && senseMid == WHITE) {
    Serial.println("::  [ALL WHITE]");
    if (forwardCount == 0) {
      if(lastTurn == LEFT) {
        turnLeft();
      }
      if(lastTurn == RIGHT) {
        turnRight();
      }
      if(lastTurn == STRAIGHT) {
        goStraight();
      }
    }
    whiteCount++;
    if(whiteCount >= 30) {
      drive(motorRight, HALT);
      drive(motorLeft, HALT);
      Serial.println("Break");
      beep(0);
      delay(1000);
      beep(0);
      delay(1000);
      beep(1);
      while(true);
    }
  } else {
    whiteCount = 0;
    if(senseLeft == BLACK && senseRight == WHITE) {
      Serial.println("::   CASE1");
      turnLeft();
    } else if(senseLeft == WHITE && senseRight == BLACK) {
      Serial.println("::   CASE2");
      turnRight();
    } else if(senseLeft == BLACK && senseRight == BLACK) {
      Serial.println("::   CASE3");
      goStraight();
    } else if(senseLeft == WHITE && senseRight == WHITE) {
      Serial.println("::   CASE4");
      goStraight();
    }
  }
  Serial.println();
}