 
 void follow() {
   while(!tagged) {
-    //happens every loop
-    move(1, 128, rightDirMod); //right motor with rightDirMod
-    move(2, 128, leftDirMod); //left motor with leftDirMod
-
-    Serial.println(leftDistance);
+    calcDistance();
+    diff = leftDistance - rightDistance;
+    diffMag = abs(diff);
+
+    if(!(leftDistance <= maximumRange && leftDistance >= minimumRange) &&
+       !(rightDistance <= maximumRange && rightDistance >=minimumRange)) {
+        found = false;
+        lost = true;
+        search();
+    }
+    
+    if(diff > 0 && diffMag > 2) {
+        leftMod = 50*(diffMag/rightDistance);
+        //Serial.println("R");
+    } else if(diff < 0 && diffMag > 2) {
+        rightMod = 50*(diffMag/leftDistance);
+        //Serial.println("L");
+    } else {
+        leftMod = 50;
+        rightMod = 50;
+        //Serial.println("F");
+    }
+    leftSpeed = baseSpeed + leftMod;
+    rightSpeed = baseSpeed + rightMod;
+    //Serial.print("left: ");
+    Serial.print(leftDistance);
+    Serial.print(",");
     Serial.println(rightDistance);
     
-    //change dir
-    //0 is forward, 1 is backward
-    if((leftDistance - rightDistance) > 10 ) {
-      //if right sensor is closer, slight turn right
-      rightDirMod = 0;
-      leftDirMod = 1;
-    } else if((leftDistance - rightDistance) < -10) {
-      //if left sensor is closer, slight turn left
-      rightDirMod = 1;
-      leftDirMod = 0;
-    } else {
-      rightDirMod = 0;
-      leftDirMod = 0;
-    }
-    calcDistance();
-    if((leftDistance <= 5 && leftDistance >= minimumRange) ||
-         (rightDistance <= 5 && rightDistance >=minimumRange)) {
-          tagged = true;
-          Serial.println("GOTCHA");
-        stop();  
-    }
-    delay(10);
-  } 
-  while(true) {
-    stop();
+    move(1, leftSpeed, 0);
+    move(2, rightSpeed, 0);
   }
 }
 

#define rightTrig 6 // right Trigger Pin

// POSSIBLY CHANGE THIS FOR ARENA SIZE
int maximumRange = 20; // Maximum range needed
int minimumRange = 0; // Minimum range needed
long duration, leftDistance, rightDistance; // Duration used to calculate distance

boolean found = false;  // used for search()

boolean tagged = false; // used for follow()
int rightDirMod = 0;
int leftDirMod = 0;

void setup(){
  Serial.begin (9600);

}

void calcDistance() {
  if (iterator == 20) {
    iterator = 0;
  }
  prevLeft = leftDistance;
  prevRight = rightDistance;
/* The following trigPin/echoPin cycle is used to determine the
 distance of the nearest object by bouncing soundwaves off of it. */ 
// digitalWrite(leftTrig, LOW); 
// delayMicroseconds(2); 
// digitalWrite(leftTrig, HIGH);
// delayMicroseconds(10); 
// digitalWrite(leftTrig, LOW);
// duration = pulseIn(leftEcho, HIGH);
 //Calculate the distance (in cm) based on the speed of sound.
 delay(10);
  digitalWrite(leftTrig, LOW);
  delayMicroseconds(5);
  digitalWrite(leftTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(leftTrig, LOW);
  pinMode(leftEcho, INPUT);
  durationLeft = pulseIn(leftEcho, HIGH);
 leftDistance = durationLeft/58.2;  

  delay(10);
 
// digitalWrite(rightTrig, LOW); 
// delayMicroseconds(2); 
// digitalWrite(rightTrig, HIGH);
// delayMicroseconds(10); 
// digitalWrite(rightTrig, LOW);
// duration = pulseIn(rightEcho, HIGH);
  digitalWrite(rightTrig, LOW);
  delayMicroseconds(5);
  digitalWrite(rightTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(rightTrig, LOW);
  pinMode(rightEcho, INPUT);
  durationRight = pulseIn(rightEcho, HIGH);
 //Calculate the distance (in cm) based on the speed of sound.
 rightDistance = durationRight/58.2;  

// if((leftDistance<minimumRange)||(leftDistance>maximumRange)) {
//  leftDistance = prevLeft;
// }
// if((rightDistance<minimumRange)||(rightDistance>maximumRange)) {
//  rightDistance = prevRight;
// }

//button
//esc code
 
// iterator++;
// leftDistance = 0;
// rightDistance = 0;
// for (int j= 0; j < 20; j++) {
//  leftDistance += lD[j];
//  rightDistance += rD[j];
// }
// leftDistance = leftDistance/20;
// rightDistance = rightDistance/20;
}

/*
