   hDrive(0.0, 0.0, 0.0);
 }
 
+void extinguish() {
+  for(int i = 0; i <= 180; i += interval){
+    extingusher_servo.write(i);
+    delay(15*interval);//give time for servo to move
+
+    if(!digitalRead(flame_sensor))
+      digitalWrite(FLAME_LED, HIGH);
+  
+    //turn on fan
+    fan_motor.set(1);
+    while(!digitalRead(flame_sensor)){
+      //while there is a fire
+      delay(1000);   
+    }
+    //turn off fan
+    fan_motor.set(0);
+    digitalWrite(FLAME_LED, LOW);  
+  }
+  
+  for(int i = 180; i >=0; i -= interval){
+    extingusher_servo.write(i);
+    delay(15*interval);//give time for servo to move
+
+    if(!digitalRead(flame_sensor))
+      digitalWrite(FLAME_LED, HIGH);
+  
+    //turn on fan
+    fan_motor.set(1);
+    while(!digitalRead(flame_sensor)){
+      //while there is a fire
+      delay(1000);   
+    }
+    //turn off fan
+    fan_motor.set(0);
+    digitalWrite(FLAME_LED, LOW);  
+  }
+}
+

  attachInterrupt(4, countPulseLeft, CHANGE);
  
  // Sound Sensor
  pinMode(ledPin, OUTPUT);
  pinMode(sensorPin, INPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
  if (soundStart() || digitalRead(13)) {
    robotOn = !robotOn;
  }

  if(robotOn){
//    Serial.println(pulseRight);
//    definedStartSearch();
     digitalWrite(SOUND_LED, HIGH);
	  digitalWrite(FLAME_LED, HIGH);
	  delay(1000);
	 digitalWrite(SOUND_LED, LOW);
	  digitalWrite(FLAME_LED, LOW);
	  delay(1000);
  }
  else{
    hDrive(0.0, 0.0, 0.0);

#define SOUND_LED 52

#define WALL_DISTANCE 7

#define flame_sensor 30
#define servo_pin 31
#define interval 10

Motor fan_motor(50,51);
Servo extingusher_servo;

Motor leftMotor1(3, 4);
Motor leftMotor2(5, 6);
