 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)
+const int SpeedL = 230; // Default speed
+const int SpeedR = 230;
 
 //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 black = 700; // black left < 520, right < 650
+const int green = 700;  //same^ 690 - 770
+// white is > 830
 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);
   

//  Serial.println(prR);
  
  //determine if we hit a line and what angle
  if(prL > black && prR > black) {
    //black for both
  } else if(prL > black && prR <= black) {
    //black for left, white for right
  } else if(prL <= black && prR > black) {
    //white for left, black for right
  } else {
    //white for both 
  }
  
  //if hit line, what do?
  //How know if green?
}

void moveForward() {
  analogWrite(speedL, SpeedL);
  analogWrite(speedR, SpeedR);
  digitalWrite(enLForPin, HIGH);
  digitalWrite(enLRevPin, LOW);
  digitalWrite(enRForPin, HIGH);
  digitalWrite(enRRevPin, LOW);
}

void moveBackward() {
  analogWrite(speedL, SpeedL);
  analogWrite(speedR, SpeedR);
  digitalWrite(enLForPin, LOW);
  digitalWrite(enLRevPin, HIGH);
  digitalWrite(enRForPin, LOW);
  digitalWrite(enRRevPin, HIGH);
}

void moveRight() {
  analogWrite(speedL, SpeedL);
  analogWrite(speedR, SpeedR);
  digitalWrite(enRForPin, HIGH);
  digitalWrite(enRRevPin, LOW);
  digitalWrite(enLForPin, LOW);
  digitalWrite(enLRevPin, HIGH);
}

void moveLeft() {
  analogWrite(speedL, SpeedL);
  analogWrite(speedR, SpeedR);
  digitalWrite(enRForPin, LOW);
  digitalWrite(enRRevPin, HIGH);
  digitalWrite(enLForPin, HIGH);
  digitalWrite(enLRevPin, LOW);
}

void moveStop() {
  digitalWrite(speedL, LOW);
  digitalWrite(speedR, LOW);
}

void quarterRight() {
  moveRight();
  delay(220);
}

void quarterLeft() {
  moveLeft();
  delay(250);
}

void turnaroundL() {
  moveLeft();
  delay(1000);
}

void turnaroundR() {
  moveRight();
  delay(1000);
}

boolean isRockAhead() {
  if (dis > 45) {
    return false;
  }
  return true;
}



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