 #include "servo.h"
 
 #define WALL_DISTANCE 5
+
+#define servo_pin 45
+#define fire_sensor 47
 
 Motor leftMotor1(2, 3);
 Motor leftMotor2(4, 5);

  }
}


Motor strafeMotor(10, 11);

ColorSensor colour_sensor(44, 46, 48, 50, 52);

Ultrasonic frontUltrasonic(22, 23, true);
Ultrasonic leftUltrasonic(24, 25, true);
Ultrasonic rightUltrasonic(26, 27, true);
Ultrasonic backUltrasonic(28, 29, true);

// Drivetrain commands
void driveToWhite();
void driveThroughRoom();
void hDrive(double move, double rotate, double strafe);
void definedStartSearch();

// Extinguisher commands
void scanForFire();

void setup() {
  Serial.begin(9600);
  pinMode(servo_pin, OUTPUT);
  pinMode(fire_sensor, INPUT);
}
 
int btn = 0;

void loop() {
  // put your main code here, to run repeatedly:
  if(digitalRead(21)){
    delay(100);
    
    if(!btn){
      delay(100);
      
      driveToWhite()
    }
    else{
      hDrive(0.0, 0.0, 0.0);
    }

    btn = !btn;
  }
}

