/*------ Arduino Fire Fighting Robot Code----- */
  #include <Servo.h>
  Servo myservo;
  int pos = 0;    
  boolean fire = false;
  /*-------defining Inputs------*/
  #define Left_Sensor A0      // left sensor
  #define Right_Sensor A1      // right sensor
  #define Forward_Sensor A2   //forward sensor
  /*-------defining Outputs------*/
  #define LeftMotor1 7       // left motor
  #define LeftMotor2 8       // left motor
  #define enA 5
  #define RightMotor1 9       // right motor
  #define RightMotor2 10       // right motor
  #define enB 6
  #define pump 12
  int speed = 120 ; //  speed of motor1 (0-255)
  
  void setup()
  {
    pinMode(Left_Sensor, INPUT);
    pinMode(Right_Sensor, INPUT);
    pinMode(Forward_Sensor, INPUT);
    pinMode(LeftMotor1, OUTPUT);
    pinMode(LeftMotor2, OUTPUT);
    pinMode(enA, OUTPUT);
    pinMode(RightMotor1, OUTPUT);
    pinMode(RightMotor2, OUTPUT);
    pinMode(enB, OUTPUT);
    pinMode(pump, OUTPUT);
    myservo.attach(11);
    myservo.write(90); 
  }
  void put_off_fire()
  {
      delay (500);
      digitalWrite(LeftMotor1, HIGH);
      digitalWrite(LeftMotor2, HIGH);
      digitalWrite(RightMotor1, HIGH);
      digitalWrite(RightMotor2, HIGH);
      analogWrite(enA, 0);    
      analogWrite(enB, 0);   
      digitalWrite(pump, HIGH);
      delay(500);
      for (pos = 90; pos >= 50; pos -= 1) { 
      myservo.write(pos); 
      delay(10);  
    }
      for (pos = 50; pos <= 130; pos += 1) { 
      myservo.write(pos); 
      delay(10);  
    }
    for (pos = 130; pos >= 50; pos -= 1) { 
      myservo.write(pos); 
      delay(10);
    }
    digitalWrite(pump,LOW);
      for (pos = 50; pos <= 90; pos += 1) { 
      myservo.write(pos); 
      delay(10);  
    }
    //myservo.write(90);
    fire=false;
  }
  void loop()
  {
    myservo.write(90); //Sweep_Servo();  
      if (digitalRead(Left_Sensor) ==1 && digitalRead(Right_Sensor)==1 && digitalRead(Forward_Sensor) ==1) //If no fire is detect all sensors are zero
      {
      //Do not move the robot
      digitalWrite(LeftMotor1, HIGH);
      digitalWrite(LeftMotor2, HIGH);
      digitalWrite(RightMotor1, HIGH);
      digitalWrite(RightMotor2, HIGH);
      analogWrite(enA, 0);
      analogWrite(enB, 0);
      }
      else if (digitalRead(Forward_Sensor) ==0) //If Fire is detected straight ahead
      {
      //Move the robot forward
      digitalWrite(LeftMotor1, HIGH);
      digitalWrite(LeftMotor2,LOW);
      analogWrite(enA,speed);
      digitalWrite(RightMotor1, HIGH);
      digitalWrite(RightMotor2, LOW);
      analogWrite(enB,speed);
      fire = true;
      }
      else if (digitalRead(Left_Sensor) ==0) //If Fire is detected to the left
      {
      //Move the robot left
      digitalWrite(LeftMotor1, HIGH);
      digitalWrite(LeftMotor2, LOW);
      analogWrite(enA, speed);
      digitalWrite(RightMotor1, HIGH);
      digitalWrite(RightMotor2, HIGH);
      analogWrite(enB, 0 );
      fire = true;
      }
      else if (digitalRead(Right_Sensor) ==0) //If Fire is detected to the right
      {
      //Move the robot right
      digitalWrite(LeftMotor1, HIGH);
      digitalWrite(LeftMotor2, HIGH);
      analogWrite(enA, 0);
      digitalWrite(RightMotor1, HIGH);
      digitalWrite(RightMotor2, LOW);
      analogWrite(enB, speed);
      fire = true;
      }
  delay(300); //Slow down the speed of robot
      while (fire == true)
      {
        put_off_fire();
      }
  }