/*
  This is a mazebot that will basically follow every left turn, spin in dead ends, 
  and come to a halt if it finds itself in open space
  
  Select front sensor and adjust slider to simulate approaching dead end.
  The motors will spin opposite to each other until front sensor detects no blockage.

  Select left sensor and move the slider outside of 15 +/- 3 for error (see distance limits)

  Move both to max range and this simulates exiting the maze...
  ...or your sensors are not being echo'd back to.
*/

//Left Motor
const int l_en = 12;
const int l_step = 5;
const int l_dir = 4;

// Right Motor
const int r_en = 13;
const int r_step = 3;
const int r_dir = 2;

int delMS = 800;

// Sonic sensors
const int s_left_T = A1;
const int s_left_E = A0;
const int s_front_T = A2;
const int s_front_E = A3;

// Sensor initial values
int df = 0;
int dl = 0;

// Distance limits
int near = 10; // How close to stay to the wall
int far = 350;  // Far enough to indicate we have exited the maze
int err = 3;  // Allowable tracking error

long readSensor(int trig, int echo)
{
  long dur;
  digitalWrite(trig, LOW);
  delayMicroseconds(5);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  dur = pulseIn(echo, HIGH);
  return (dur / 57);
}

void motFwd() {
  Serial.println("Moving forward..");
  digitalWrite(l_en, LOW);
  digitalWrite(r_en, LOW);
  digitalWrite(r_step, HIGH);
  digitalWrite(l_step, HIGH);
  delayMicroseconds(delMS);
  digitalWrite(r_step, LOW);
  digitalWrite(l_step, LOW);
  delayMicroseconds(delMS);
}
void motLeft() {
  Serial.println("Adjusting left..");
  digitalWrite(l_en, HIGH);
  digitalWrite(r_step, HIGH);
  delayMicroseconds(delMS);
  digitalWrite(r_step, LOW);
  delayMicroseconds(delMS);
}
void motRight() {
  Serial.println("Adjusting right..");
  digitalWrite(r_en, HIGH);
  digitalWrite(l_step, HIGH);
  delayMicroseconds(delMS);
  digitalWrite(l_step, LOW);
  delayMicroseconds(delMS);
}
void motStop() {
  Serial.println("Dead end, spinning..");
  digitalWrite(r_dir, LOW);
  while (readSensor(s_front_T, s_front_E) < (near - 2) && readSensor(s_front_T, s_front_E) < (near - 2)) {
    digitalWrite(r_step, HIGH);
    digitalWrite(l_step, HIGH);
    delayMicroseconds(delMS);
    digitalWrite(r_step, LOW);
    digitalWrite(l_step, LOW);
    delayMicroseconds(delMS);
  }
  digitalWrite(r_dir, HIGH);
  motFwd();
}

void setup() {
  pinMode(l_en, OUTPUT);
  pinMode(l_step, OUTPUT);
  pinMode(l_dir, OUTPUT);
  pinMode(r_en, OUTPUT);
  pinMode(r_step, OUTPUT);
  pinMode(r_dir, OUTPUT);

  digitalWrite(l_en, LOW);
  digitalWrite(l_dir, HIGH);
  digitalWrite(r_en, LOW);
  digitalWrite(r_dir, HIGH);


  pinMode(s_left_T, OUTPUT);
  pinMode(s_front_T, OUTPUT);
  pinMode(s_left_E, INPUT);
  pinMode(s_front_E, INPUT);

  Serial.begin(9600);
}

void loop() {
  df = readSensor(s_front_T, s_front_E);
  dl = readSensor(s_left_T, s_left_E);

  // Are we stil in the maze?
  if (df > far && dl > far) {
    Serial.println("I have escaped your crazy maze..");
    delay(500);
    digitalWrite(r_step, LOW);
    digitalWrite(l_step, LOW);
    digitalWrite(r_en, LOW);
    digitalWrite(l_en, LOW);
    exit(0);
  }

  if (df < near) {  // Have we hit a dead end?
    motStop();
  } else  
    if (dl > near + err) { // Are we too far away from the left wall
      motLeft();
    } else  
      if (dl < near - err) { // Are we too close to the left wall?
        motRight();
      } else
      { motFwd(); // All is good..keep going forward
      }

  //Serial.print("Front: ");
  //Serial.print(df);
  //Serial.print("\tLeft: ");
  //Serial.println(dl);
}