/* this program is designed to go until it is a foot away from a wall, then look for the highest distance,
and then turn in that direction. It not only does 90 degrees left and right, but all degrees.
*/

#include <Servo.h> //someone's servo library
Servo pathFinder; //the name for my servo; servos are defined differently.

int pos = 90; //position of the servo, 90 is straight ahead
int posL = 0; //used later, the position in which the distance was greatest.

const int enableR = 10; //enable pins for the motors
const int enableL = 11;

const int motorR1 = 9;  //control pins
const int motorR2 = 8;  //for right motor

const int motorL1 = 7; //control pins
const int motorL2 = 6; //for left motor

const int echoPin = 2; //ultrasonic stuff
const int trigPin = 4;
const int threshold = 12;  //threshold distance from wall
int duration, cm;

int cmL = 0; //value of the highest distance seen by the scan

long steering = 0; //time variable to determine how much to turn left or right
int multiplier = 9; // a multiplier for the steering because nobody is perfect

void setup() {    //sets up pins as outputs and inputs
 
  pathFinder.attach(12); //set servo to pin 12
 
  pinMode(enableR, OUTPUT); //we all know what this is
  pinMode(enableL, OUTPUT);
 
  pinMode(motorR1, OUTPUT);
  pinMode(motorR2, OUTPUT);
 
  pinMode(motorL1, OUTPUT);
  pinMode(motorL2, OUTPUT);
 
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
 
  analogWrite(enableR, 255);
  analogWrite(enableL, 255);
 
  pathFinder.write(90); //set the servo to straight ahead
}

void loop() {

 
  getDistance(); //get a value for cm
  forward(); //make the robot go forward
  if(cm <= threshold) { //if see an obstruction do this code
    freeze(); //stop
    sweep(); //look around and find highest distance and position of sensor at that distance
    turnToFace(); //turn to the highest distance
    delay(100); //wait so no lockups
  }
 
}

void forward() {

  analogWrite(enableR, 255);   //full speed ahead!
  analogWrite(enableL, 255);
 
  digitalWrite(motorR1, HIGH);  //turn motor forward
  digitalWrite(motorR2, LOW);
 
  digitalWrite(motorL1, HIGH);   //turn motor forward
  digitalWrite(motorL2, LOW);
}

void left() {
  analogWrite(enableR, 180); //turn left by giving left motor 50% power
  analogWrite(enableL, 255);
 
  digitalWrite(motorR1, HIGH);  //turn motor forward
  digitalWrite(motorR2, LOW);
 
  digitalWrite(motorL1, HIGH);  //turn motor forward
  digitalWrite(motorL2, LOW);
}

void right() {
  analogWrite(enableR, 255);
  analogWrite(enableL, 180); //turn right by giving right motor 50% power
 
  digitalWrite(motorR1, HIGH); //turn motor forward
  digitalWrite(motorR2, LOW);
 
  digitalWrite(motorL1, HIGH); //turn motor forward
  digitalWrite(motorL2, LOW);
}

void freeze() {
  digitalWrite(motorR1, LOW); //stop motor
  digitalWrite(motorR2, LOW);
  digitalWrite(motorL1, LOW);
  digitalWrite(motorL2, LOW);
}

void sweep() {  //sweep sensor back and forth
  pathFinder.write(0);
  for(pos = 0; pos <= 180; pos++) {
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    duration = pulseIn(echoPin, HIGH);
    cm = duration /29 /2;
    pathFinder.write(pos);
    if(cm >= cmL) {
      cmL = cm;
      posL = pos;
    }
   
    delay(10);
  }
  pathFinder.write(90);
 
}

void getDistance() {  //reads diatance
   digitalWrite(trigPin, LOW);
   delayMicroseconds(2);
   digitalWrite(trigPin, HIGH);
   delayMicroseconds(10);
   digitalWrite(trigPin, LOW);
   duration = pulseIn(echoPin, HIGH);
   cm = duration /29 /2;
}

void turnToFace() {
  steering = (abs(90 - posL)) * multiplier;
  if((90 - posL) >= 0) {
    right();
    delay(steering);
    forward();
  }
  else {
    left();
    delay(steering);
    forward();
  }
}