 #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
+int maximumRange = 150; // Maximum range needed
+int minimumRange = 1; // Minimum range needed
+long leftDistance = 1000;
+long rightDistance = 1000;
+long prevLeft;
+long prevRight;
+long durationLeft, cmLeft, inchesLeft, cmRight, inchesRight, durationRight;
+long lD[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+long rD[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+int iterator = 0;
 
 boolean found = false;  // used for search()
+boolean lost = false;
 
 boolean tagged = false; // used for follow()
 int rightDirMod = 0;
 int leftDirMod = 0;
+
+//------------------------------
+#define baseSpeed 128
+int leftMod = 0;
+int rightMod = 0;
+int leftSpeed = 0;
+int rightSpeed = 0;
+
+int speedModifier;
+int diff;
+int diffMag;
+
+int leftDir = 0;
+int rightDir = 1;
+//-----------------------------
 
 void setup(){
   Serial.begin (9600);


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);
    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();
  }
}



void loop(){
  search();
}

void move(int motor, int speed, int direction){
