   delay(100);
 }
 
+void hDrive(double move, double rotate, double strafe) {
+  double leftSpeed, rightSpeed;
+  
+  move = constrain(move, -1.0, 1.0);
+  rotate = constrain(rotate, -1.0, 1.0);
+  strafe = constrain(strafe, -1.0, 1.0);
+
+  if (move > 0.0) {
+    if (rotate > 0.0) {
+      leftSpeed =  move - rotate;
+      rightSpeed = max(move, rotate);
+    } else {
+      leftSpeed = max(move, -rotate);
+      rightSpeed = move + rotate;
+    }
+  } else {
+    if (rotate > 0.0) {
+      leftSpeed = -max(-move, rotate);
+      rightSpeed = move + rotate;
+    } else {
+      leftSpeed = move - rotate;
+      rightSpeed = -max(-move, -rotate);
+    }
+  }
+
+  leftMotor1.set(constrain(leftSpeed, -1.0, 1.0));
+  leftMotor2.set(constrain(leftSpeed, -1.0, 1.0));
+  rightMotor1.set(-constrain(rightSpeed, -1.0, 1.0));
+  rightMotor2.set(-constrain(rightSpeed, -1.0, 1.0));
+  strafeMotor.set(strafe);
+}
+
+void scanForFire(){
+  servo_pin.write(0);
+  for(int i = 0; i <= 180;i += 10){
+    if(digitalRead(fire_sensor)){
+      delay(10);//make sure that the reading isn't a false positive
+      while(digitalRead(fire_sensor))
+      {
+        //Turn on extinguisher
+        delay(50);
+      }
+    }
+    else if(val == HIGH && !directionRight)
+    {
+      directionRight = true;  //Forward
+    }
+  }
+}
+
+double getAngle() {
+  return ((pulseLeft - pulseRight) * 12.5 / 700) / 8;
+}
+
+double getPercentError(double actual, double target) {
+  return abs(actual - target) / target;
+}
+
+void turnToAngle(double targetAngle) {
+  while (abs(getAngle()) < abs(targetAngle)) {
+    hDrive(0.0, getPercentError(getAngle(), targetAngle()), 0.0);
+  }
+  hDrive(0.0, 0.0, 0.0);
+}
+
 void drivetrainCheck() {
   hDrive(0.3, 0.0, 0.0); delay(300); hDrive(-0.3, 0.0, 0.0); delay(300);
   hDrive(0.0, 0.3, 0.0); delay(300); hDrive(0.0, -0.3, 0.0); delay(300);

  }
}

void scanForFire(){
//  servo_pin.write(0);
//  for(int i = 0; i <= 180;i += 10){
//    if(digitalRead(fire_sensor)){
//      delay(10);//make sure that the reading isn't a false positive
//      while(digitalRead(fire_sensor))
//      {
//        //Turn on extinguisher
//        delay(50);
//      }
//    }
//    break;
//  }
}

void countPulseRight() {
  int Lstate = digitalRead(encoderARight);
  if((encoderALastRight == LOW) && Lstate==HIGH)

#include "Ultrasonic.h"
#include "ColorSensor.h"
#include <Servo.h>

#define WALL_DISTANCE 5
#define servo_pin 45
#define fire_sensor 47

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