 #include <Servo.h>
 #include <SharpIR.h>
 
+#define COLL_DIST 20 // Set maximum distance to avoid collision
+#define TURN_DIST COLL_DIST+10 // Sets maximum distance to turn 
+#define MAX_SPEED 200
+#define MAX_SPEED_OFFSET 10              
+
 //SERVO
 Servo myservo;           // create servo object to control a servo
 

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

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