   servoLeft.attach(SERVO_L_PIN);
   servoRight.attach(SERVO_R_PIN);
 
-  leftSum = rightSum = 0;
+  beaconLeftCount = beaconRightCount = 0;
+  initialBeaconScan();
 }
 
 void loop() {
-  irBeaconLeftVal  = analogRead(IR_BEACON_L_PIN) * analogConversionFactor;  
-  irBeaconRightVal = analogRead(IR_BEACON_R_PIN) * analogConversionFactor;
+  checkForBeacon(100);
 
-  if(irBeaconLeftVal == 0 && irBeaconRightVal > 0) {
-    leftSum++;
-  } else if(irBeaconRightVal == 0 && irBeaconLeftVal > 0) {
-    rightSum++;
-  } else if(irBeaconRightVal == 0 && irBeaconLeftVal == 0){
-    leftSum = rightSum = 0;
+  Serial.println("Left: "+String(beaconLeftVal)+"Right "+String(beaconRightVal));
+  
+  steerTowardsBeacon();
+}
+
+void steerTowardsBeacon(){
+  if(beaconLeftCount - beaconRightCount <= -2 ){
+    turnRight(servoLeft,servoRight);
+    //Serial.println("Turn right "+String(beaconLeftCount - beaconRightCount));
+  } else if (beaconLeftCount - beaconRightCount >= 2 ){
+    turnLeft(servoLeft,servoRight);
+    //Serial.println("Turn left "+String(beaconLeftCount - beaconRightCount));
+  } else { //beaconLeftCount == beaconRightCount
+    moveForward(servoLeft,servoRight);
+    //Serial.println("Move forward "+String(beaconLeftCount - beaconRightCount));
   }
-  
-  if(leftSum < rightSum){
-    turnRight(servoLeft,servoRight);
-  } else if (leftSum > rightSum){
+}
+void checkForBeacon(int milliseconds){
+  for(int i=0; i<milliseconds/10; i++){
+    beaconLeftVal  = analogRead(IR_BEACON_L_PIN) * analogConversionFactor;  
+    beaconRightVal = analogRead(IR_BEACON_R_PIN) * analogConversionFactor;
+    if(beaconLeftVal == 0 && beaconRightVal > 0) {
+      beaconLeftCount++;
+    } else if(beaconRightVal == 0 && beaconLeftVal > 0) {
+      beaconRightCount++;
+    } else if(beaconRightVal == 0 && beaconLeftVal == 0){
+      beaconLeftCount = beaconRightCount = 0;
+    }
+     delay(10);
+  }
+}
+
+void initialBeaconScan(){
+  Serial.println("Initial bacon scan!");
+  bool foundSomething = false;
+  int pauseTime = 500;
+  while(!foundSomething){
+    stopMovement(servoLeft,servoRight);
+    for(int i=0; i<pauseTime/10; i++){
+      beaconLeftVal  = analogRead(IR_BEACON_L_PIN) * analogConversionFactor;  
+      beaconRightVal = analogRead(IR_BEACON_R_PIN) * analogConversionFactor;
+      delay(10);
+      if(beaconLeftVal == 0 && beaconRightVal == 0){
+        Serial.println("Initial bacon scan complete");
+        tone(6,1000,500);
+        foundSomething = true;
+        break;
+      }
+    }
     turnLeft(servoLeft,servoRight);
-  } else { //leftSum == rightSum
-    moveForward(servoLeft,servoRight);
+    delay(100);
   } 
-  delay(100);
+  stopMovement(servoLeft,servoRight);
 }
 
 void turnLeft(Servo servoLeft, Servo servoRight){

#include <Servo.h>

const int IR_BEACON_L_PIN  = A0;
const int IR_BEACON_R_PIN = A1;
const int SERVO_L_PIN = 13;
const int SERVO_R_PIN = 12;

// Constants for servo
Servo servoRight;
Servo servoLeft;
int servoLeftForward = 1550;
int servoLeftBackward = 1450;
int servoRightForward = 1450;
int servoRightBackward = 1550;

const float analogConversionFactor = 5.0 / 1024.0;
float irBeaconLeftVal, irBeaconRightVal, tmpSum, irBeaconMeanValue;
float leftSum, rightSum;
void setup() {
  Serial.begin(9600);
  pinMode(IR_BEACON_L_PIN, INPUT);

  servoLeft.writeMicroseconds(servoLeftForward);
  servoRight.writeMicroseconds(servoRightForward);
}
void stopMovement(Servo servoLeft, Servo servoRight){
  servoLeft.writeMicroseconds(1500);
  servoRight.writeMicroseconds(1500);
}

