Our code for our Adruino based firefighting robot

Main_Program.ino

#include <LiquidCrystal.h>
#include <Servo.h> 

// initialize the library with the numbers of the interface pins
LiquidCrystal lcd(13,12,11,10,9,8);

Servo servo;

int servoPos = 0;

int lMotorFor = 4;   
int lMotorBack = 5;
int rMotorFor = 6;
int rMotorBack = 7;

int irSensor  = A0;

int fSensor = A1;
int rSensor = A2;

int rLineSensor = A5;
int lLineSensor = 1;

boolean rOn = false;
boolean lOn = false;
boolean kickBack = false;
boolean alignRev = false;
boolean rightOff = false;
boolean leftOff = false;
boolean rightMove = false;
boolean lastRoomCandle = false;
boolean isCandleOut = false;
boolean extraAdvance = false;

int rLineSensorReading = 0;
int lLineSensorReading = 0;

int fSensorReading = 0;
int rSensorReading = 0;
int irSensorReading = 0;

int initialMaintain = 0;
int initialIR = 0;

int stage = 0;

int linesPassed = 0;

int lightLayerOne = A3;
int lightLayerTwo = 2;
int lightLayerThree = A4;

//Sets the LCD text based on text and column and rows called.
void setLCDText(String text,int col=0,int row=0) {
  lcd.setCursor(col,row);
  lcd.print(text); 
}
//Utilizing the setLCDText function to set the first and second rows text on the lcd.
void setText(String row1, String row2 = ""){ 
  lcd.clear();
  setLCDText(row1,0,0);
  setLCDText(row2,0,2);
}

//Sets the motors to off.
void off() {
  digitalWrite(rMotorFor,0); 
  digitalWrite(lMotorFor,0); 
  digitalWrite(rMotorBack,0); 
  digitalWrite(lMotorBack,0); 
}
//Turns on the right and left motor forward
void forward() {
  digitalWrite(rMotorFor,1); 
  digitalWrite(lMotorFor,1); 
  digitalWrite(rMotorBack,0); 
  digitalWrite(lMotorBack,0); 
}
//Turns on the right and left motor backwards.
void backward(){
  digitalWrite(rMotorFor,0); 
  digitalWrite(lMotorFor,0); 
  digitalWrite(rMotorBack,1); 
  digitalWrite(lMotorBack,1);  
}
//Turn robot clockwise using on forward power, no back.
void clockwise(){
  digitalWrite(rMotorFor,0); 
  digitalWrite(lMotorFor,1); 
  digitalWrite(rMotorBack,0); 
  digitalWrite(lMotorBack,0);   
}
//Turns robot clockwise using both one wheel forward and the other backward!
void twoWheelClockwise() {
  digitalWrite(rMotorFor,0); 
  digitalWrite(lMotorFor,1); 
  digitalWrite(rMotorBack,1); 
  digitalWrite(lMotorBack,0);   
}
//Turns counterclockwise using both wheels to turn.
void twoWheelCounterClockwise() {
  digitalWrite(rMotorFor,1); 
  digitalWrite(lMotorFor,0); 
  digitalWrite(rMotorBack,0); 
  digitalWrite(lMotorBack,1);   
}
//Turns counterclockwise only using forward power
void counterClockwise(){
  digitalWrite(rMotorFor,1); 
  digitalWrite(lMotorFor,0); 
  digitalWrite(rMotorBack,0); 
  digitalWrite(lMotorBack,0);   
}

//Reads in all the sensor values into variables. 
void sensorCheck(){
   fSensorReading = analogRead(fSensor);
   rSensorReading = analogRead(rSensor);  
   irSensorReading = analogRead(irSensor);
   
   rLineSensorReading = digitalRead(rLineSensor);
   lLineSensorReading = digitalRead(lLineSensor);
}

//Records the initial values of some of the sensors in which initial values are needed.
void initialChecks() {
  initialMaintain = rSensorReading;
  initialIR = irSensorReading;
}
//Wall hugger function. Maintains the distance within 50 from the wall. 
void checkBounds() {
  if((initialMaintain-50) > rSensorReading) {
    clockwise();
    delay(25);
    setText("Too far","from left");
    forward(); 
  }
  if((initialMaintain+50) < rSensorReading) {
    counterClockwise();
    delay(25);
    setText("Too far","from right");
    forward(); 
  }
}
//Checks the front to ensure we are not too close to a wall in the front. If we are we should turn left 
//(at this point we know rightwall is also blocked so are only likely free spot is left)
void checkFront() {
  if(fSensorReading > 300) {
    counterClockwise();
    delay(250);
  } 
}
//Checks if the lineSensors are on white lines or not. If they are they set corresponding variables.
void lineSensorTest() {
  if(rLineSensorReading == 1 && rOn == false){
    setText("RIGHT");
    rOn = true;
    return;
  }
 
  if(lLineSensorReading == 1 && lOn == false) {
    setText("LEFT");
    lOn = true;
    return;
  }
}

//Scans and adjusts to lowest candle value and then attempts to put out the candle once found. 
void candleScan() {
//Do 180 degree scan for candle. Record lowest value. 
//Once finished scan go back until the recorded value is found again
//Use a +-10 with the recorded value as it may not be exactly the same when
//moving back.
  setText("Scanning for", "Candle");
  int lowestVal = irSensorReading;
  
  //Scan back and forth to find lowest values
  for(int x = 0;x < 6;x++) {
    sensorCheck();
    if(lowestVal > irSensorReading) {
      lowestVal = irSensorReading; 
    }
    twoWheelClockwise();
    delay(100);
  }
  for(int x = 0;x < 12;x++) {
    sensorCheck();
    if(lowestVal > irSensorReading) {
      lowestVal = irSensorReading; 
    }
    twoWheelCounterClockwise();
    delay(100);
  }
  setText("Lowest Value:",(String) lowestVal);
  //Turn back to lowest value found
  while(!((lowestVal-45) < irSensorReading && (lowestVal+45) > irSensorReading)) {
    sensorCheck();
    setText("LowVal Scan",(String) irSensorReading);
    twoWheelClockwise();
    delay(10);
  }
  setText("Lowval result",(String) irSensorReading);
  twoWheelCounterClockwise();
  delay(50);
  off();
  rOn = false;
  lOn = false;
  //Go forward until hit line (should be white circle around candle)
  while(rOn == false && lOn == false) {
    sensorCheck();
    lineSensorTest();
    forward();
    setText("Forward");
  }
  setText("Center on line");
  //Align better on the white line.
  while(rOn == false || lOn == false) {
    if(lOn == true && rOn == false) {
      counterClockwise();
    } 
    else if (rOn == true && lOn == false) {
      clockwise();
    }
    sensorCheck();
    lineSensorTest();
  }
  backward();
  delay(300);
  forward();
  delay(20);
  off();
  setText("Aligned", "hopefully");
  sensorCheck();
  int i = 0;
  //If candle is still there use arm and try to put it out. Moving slightly left and right.
  while(irSensorReading < 350) {
    setText("Lowering Arm");
    delay(1000);
    servo.write(48);
    delay(1500);
    setText("Raising arm");
    servo.write(170);
    sensorCheck();
    if(i < 5) {
      twoWheelCounterClockwise();
      delay(70);
    } else if(i >= 5) {
      twoWheelClockwise();
      delay(70); 
    }
    off();
    i++;
    if(i > 14)
      i = 0;
  }
  backward();
  delay(1500);
  twoWheelCounterClockwise();
  delay(1200);
  off();
  setText("Hopefully candle", "is out now!");
  isCandleOut = true;
  lastRoomCandle = false;
}

//Method called at the start of the program to show cool lights off.
void loading() {
  setText("Loading");
  digitalWrite(lightLayerThree,1); 
  delay(1000);
  digitalWrite(lightLayerTwo,1);
  setText("Loading","....");
  delay(1000);
  digitalWrite(lightLayerOne,1);
  setText("Loading",".............");
}
void animation(int stage = 0) {
  if(stage < 1000) {
    digitalWrite(lightLayerThree,0);
    digitalWrite(lightLayerTwo,0);
    digitalWrite(lightLayerOne,1);
  } else if(1000 <= stage < 2000){
    digitalWrite(lightLayerThree,0);
    digitalWrite(lightLayerTwo,1);
    digitalWrite(lightLayerOne,1); 
  } else if(2000 <= stage < 3000) {
    digitalWrite(lightLayerThree,1);
    digitalWrite(lightLayerTwo,1);
    digitalWrite(lightLayerOne,1);  
  } else if(3000 <= stage < 4000) {
    digitalWrite(lightLayerThree,0);
    digitalWrite(lightLayerTwo,0);
    digitalWrite(lightLayerOne,0);  
  } else if(4000 <= stage < 5000) {
    digitalWrite(lightLayerThree,1);
    digitalWrite(lightLayerTwo,1);
    digitalWrite(lightLayerOne,1);  
  } else if(5000 <= stage < 6000) {
    digitalWrite(lightLayerThree,0);
    digitalWrite(lightLayerTwo,0);
    digitalWrite(lightLayerOne,0);  
  } else if(6000 <= stage < 7000) {
    digitalWrite(lightLayerThree,1);
    digitalWrite(lightLayerTwo,1);
    digitalWrite(lightLayerOne,0);  
  } else if(7000 <= stage < 8000) {
    digitalWrite(lightLayerThree,0);
    digitalWrite(lightLayerTwo,0);
    digitalWrite(lightLayerOne,0);  
  } else if(8000 <= stage < 9000) {
    digitalWrite(lightLayerThree,1);
    digitalWrite(lightLayerTwo,0);
    digitalWrite(lightLayerOne,0);  
  } else if(9000 <= stage < 10000) {
    digitalWrite(lightLayerThree,0);
    digitalWrite(lightLayerTwo,0);
    digitalWrite(lightLayerOne,0);  
  } else if(10000 <= stage < 11000) {
    digitalWrite(lightLayerThree,1);
    digitalWrite(lightLayerTwo,1);
    digitalWrite(lightLayerOne,1);  
  } else if(11000 <= stage < 12000) {
    digitalWrite(lightLayerThree,0);
    digitalWrite(lightLayerTwo,0);
    digitalWrite(lightLayerOne,0);  
  } else if(12000<= stage < 13000) {
    digitalWrite(lightLayerThree,1);
    digitalWrite(lightLayerTwo,1);
    digitalWrite(lightLayerOne,1);  
  }
  
}

//Setting up all the pins and motors.
void setup() {
  pinMode(rMotorFor,OUTPUT);
  pinMode(lMotorFor,OUTPUT);
  pinMode(rMotorBack,OUTPUT);
  pinMode(lMotorBack,OUTPUT);
  pinMode(rLineSensor,INPUT);
  pinMode(lLineSensor,INPUT);
  
  pinMode(lightLayerOne,OUTPUT);
  pinMode(lightLayerTwo,OUTPUT);
  pinMode(lightLayerThree,OUTPUT);
  
  pinMode(3,OUTPUT);
  
    // set up the LCD's number of columns and rows: 
  lcd.begin(16, 2);
  
  servo.attach(3);
  servo.write(125);
  //servo.write(53);
  off();
  sensorCheck();
  //Record initial checks.
  initialChecks();
  setText("Ini maintain:",(String) initialMaintain);
  
  loading();
  //linesPassed = 5;
}

void loop() {  
  //Record sensor readings
  sensorCheck();
  lineSensorTest();

  //Go into last room if candle detected otherwise don't.
  if(linesPassed >= 6 && (initialIR-300) > irSensorReading && isCandleOut == false) {
    lastRoomCandle = true;
  }
  //If IR is low enough and in room perform the candle scan.
  else if(linesPassed != 0 && linesPassed % 2 != 0 && (initialIR-300) > irSensorReading && isCandleOut == false) {
    candleScan();
  //If no candle picked up when entering room do a scan for candle and head toward it then scan.
  } else if(linesPassed != 0 && linesPassed % 2 != 0 && extraAdvance == false && isCandleOut == false) {
      forward();
      delay(500);
      for(int x = 0;x < 6;x++) {
        twoWheelClockwise();
        sensorCheck();
        if((initialIR-200) > irSensorReading){
          candleScan();
          return;
        }
        delay(100);
      }
      for(int x = 0;x < 12;x++) {
        twoWheelCounterClockwise();
        sensorCheck();
        if((initialIR-200) > irSensorReading){
          candleScan();
          return;
        }
        delay(100);
      }
      for(int x = 0;x < 8;x++) {
        twoWheelClockwise();
        sensorCheck();
        if((initialIR-200) > irSensorReading){
          candleScan();
          linesPassed++;
          return;
        }
        delay(100);
      }
      backward();
      delay(2000);
      if(linesPassed==5) 
        twoWheelCounterClockwise(); 
      else
        twoWheelClockwise();
      delay(750);
      forward();
      linesPassed++;
  }
  //If nothing else is true then do normal routine of wall hugging and line checking.
  else {
    //If candle is out or we are not to the last room yet then do normal wall hugging.
    if(isCandleOut == true || lastRoomCandle == false) {
        checkBounds();
        checkFront();
    }
    //If we are to the last room go forward.
    if(linesPassed == 6 && lastRoomCandle == false && extraAdvance == false) {
      forward();
      delay(1500);
      extraAdvance = true;
    }
    //If candle in last room then go in and find candle.
    if(lastRoomCandle) {
      while(rOn == false && lOn == false) {
         forward();
         lineSensorTest();         
       }
      while(rOn == false || lOn == false) {
        if(lOn == true && rOn == false) {
          counterClockwise();
        } 
        else if (rOn == true && lOn == false) {
          clockwise();
        }
        lineSensorTest();
       }
       forward();
       delay(500);
       candleScan();
    }
    //Aligning on the line code below.
    if(kickBack == false && (rOn == true || lOn == true)) {
      backward();
      delay(50);
      kickBack = true;
    }
    
    else if(rOn == true && lOn == true) {
      rOn = false;
      lOn = false;
      kickBack = false;
      rightMove = false;
      linesPassed++;
      setText("Lines Passed",(String) linesPassed);
      forward();
      delay(500);
    }
    else if(lOn == true) {
      setText("C-CLOCKWISE");
      counterClockwise();
    }
    else if(rOn == true && rightMove == false) {
      linesPassed++;
      setText("Lines Passed",(String) linesPassed);
      forward();
      delay(750);
      clockwise();
      rightMove = true;
    }
    else if(rOn == true && rightMove == true) {
      clockwise();
    }
    else
      forward();
  }
  //animation handler.
  stage++;
  if(stage > 13000)
    stage = 0;
  animation(stage);
}