       gyroResetValZ += getAngle('z');
       break;
   }
+}

  return (setpoint - getAngle('x')) * kP;
}

void extinguish() {
  for(int i = 40; i <= 160; 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 = 140; i >=20; 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);  
  }
}

bool direction = true;
int leftTurns = 0;

#define WALL_DISTANCE 7

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

#define LOW_START 3230
#define HIGH_START 4370
