   pinMode(speedR, OUTPUT);
   pinMode(enRForPin, OUTPUT);
   pinMode(enRRevPin, OUTPUT);
-
+  
   //Serial
   Serial.begin(9600);
+  //Servo
+   myservo.attach(servoPin); // attaches the servo on pin 9 to the servo object
+   myservo.write(90); // tells the servo to face forward
+   delay(2000); // delay for 2 seconds
+   checkPath(); // Find the best path to travel
+   myservo.write(90); // make sure facing forward
+   moveForward(); // move forward
 } 
  
 void loop() //might need reordering of handling
 { 
-  //do servo things
-  handleServo();
-  //measure distance from sharpIR
-  dis = sharp.distance();
-  Serial.println(dis);
-  
-//  if (dis < 30 && dis > 20) {
-//    if(count == 3) {
-//      moveLeft();
-//      delay(2000);
-//    } else {
-//      count++;
-//    }
-//    return;
-//  } 
-
-    if (dis > 30 || dis < 0) {
-      moveForward();
-    } else {
-      moveStop();
+  
+   checkPath();
+  //check photoresistors
+//  Serial.println(isBlack);
+   moveForward();
+}
+
+void checkPath() {
+  int curLeft = 0;
+  int curFront = 0;
+  int curRight = 0;
+  int curDist = 0;
+  boolean isBlack = handlePhotores();
+  if (isBlack) {
+    moveStop();
+    delay(500);
+    moveBackward();
+    delay(500);
+    turnaround();
   }
-  
-  //check photoresistors
-  //handlePhotores();
-}
-
-void handleWheels() {
-  if (dis > 30) {
-    moveForward();
-    return;
-  } else {
-    moveLeft();
-    delay(250);
-    return;
-  }
-}
-
-void handleServo() {
-  //oscillates between servoMax and servoMin
-  if(servoPos > servoMax) {
-    servoInc = -1;
-  } else if(servoPos < servoMin) {
-    servoInc = 1;
-  }
-  
-  servoPos += servoInc * servoSpeed;
-  myservo.write(servoPos);
-}
-
-void handlePhotores() {
+    curDist = sharp.distance();
+    int counter;
+    for (counter = 0; counter < 5; counter++) {
+      curDist = sharp.distan
+    }
+    
+    Serial.println(curDist);
+    if (curDist < COLL_DIST){
+      checkCourse(); 
+    }
+    if (curDist < TURN_DIST) { // If cannot turn
+    	changePath();
+    }
+    if (curDist > COLL_DIST || curDist < 0) {
+    	angMax = pos;
+    }
+    if (curDist > curLeft && pos > 90) {
+    	curLeft = curDist;
+    }
+    if (curDist > curFront && pos == 90) {
+    	curFront = curDist;
+    }
+    if (curDist > curRight && pos < 90) {
+    	curRight = curDist;
+    }
+    leftMax = curLeft;
+    rightMax = curRight;
+    frontMax = curFront;
+}
+
+void setCourse() { // Sets direction
+	if (angMax < 90) {
+		moveRight();
+	} 
+	if (angMax > 90) {
+		moveLeft();
+	}
+	leftMax = 0;
+	rightMax = 0;
+	frontMax = 0;
+}
+
+void checkCourse() {
+	moveBackward();
+  delay(500);
+	moveStop();
+	setCourse();
+}
+
+void changePath() {
+	if (pos < 90) {
+		veerLeft();
+	}
+	if (pos > 90) {
+		veerRight();
+	}
+}
+
+// void handleServo() {
+//   //oscillates between servoMax and servoMin
+//   if(servoPos > servoMax) {
+//     servoInc = -1;
+//   } else if(servoPos < servoMin) {
+//     servoInc = 1;
+//   }
+  
+//   servoPos += servoInc * servoSpeed;
+//   myservo.write(servoPos);
+// }
+
+boolean handlePhotores() {
   //get values from both.
   int prL = analogRead(prLPin);
   int prR = analogRead(prRPin);

const int speedL = 4;    //speed pin for right motor (ENB)
const int enLForPin = 2;   //controller pin 1 for right motor (IN4)
const int enLRevPin = 3;   //controller pin 2 for right motor (IN3)
const int SpeedL = 250; // Default speed
const int SpeedR = 200;

int wheelTurn = 0; //Magnitude of turn, I guess could be difference of wheels
                   //(- is left and + is right and 0 is straight)
int wheelSpeed = 0;    //speed of actual robots, dependent on wheel (0-255)

//PHOTORES
#define prLPin A3        //Analog pin for right photoresisotr
#define prRPin A2        //Analog pin for left photoresistor
const int black = 100; //to be experimentally determined what is black
const int green = 60;  //same^
const int ledPin = 12;  //pin for the led

int count = 0;

enum state{
  FIND_A_WALL,
  HUG_THE_WALL,
  FIND_THE_WALL
};

state currState = FIND_A_WALL;
 
void setup() 
{ 
  //Servo
  myservo.attach(servoPin); // attaches the servo on pin 9 to the servo object 
  
  //SharpIR
  pinMode(ir, INPUT);
  

SharpIR sharp(ir, 10, 90, model); // creates a distance sensor object

int dis = 0;        //var to hold reading from SharpIR sensor
int disMax = 0;  //max value of obj to be determined "in way"
int angMax = 0;  //max value of angle to be determined "in way"
int rightMax = 0;
int leftMax = 0;
int frontMax = 0;
int course = 0;
int pos = 0;
int speedSet = 0;

//WHEELS
//left motor
