//[<o>]MINDS-i Projects. mymindsi.com 
//created by MINDS-i 20 Dec 2011
#include <Servo.h>
#include <MINDSi.h>

Servo drive, frontsteer, backsteer;            //create servo instances for servos and speed controller
int left, right, front, leftc, rightc, frontc; //create variables to store ping sensor readings
unsigned int starttime;
int frontcenter = 90;                          //tune midpoint for front and back 
int backcenter = 90;                           //to drive strait and tune forward/reverse speed
int fwdspeed = 108;
int revspeed = 75;
int turn = 35;
/*
This example will drive until it sees a wall in front of it and sends a signal to stop.
Then it will back up in the direction with more space until it sees room to move forward. 
It also steers away from walls when approaching at an angle.
Setup:
1. Attach the sensors for backing up to pins 13 and 12 (no specific order).
2. Attach the drive speed controller to pin 2.
3. Attach the front steering servo to pin 3.
4. Attach the back steering servo to pin 4 (for 4 wheel steering only).
5. Attach the front ping sensor to pin 5; the right ping sensor to 6; and the left ping sensor to 7.
 */
void setup() {
  pinMode(13, INPUT);                         //set pins we will get backing sensor data from as inputs
  pinMode(12, INPUT);

  drive.attach(4);                           //set pins the servos/speed controller will be attached to
  frontsteer.attach(5);
  backsteer.attach(6);

  frontsteer.write(frontcenter);             //move steering servos to center
  backsteer.write(backcenter);

  drive.write(90);                         //arm speed controller with nuetral signal for half a second
  delay(500);                           
}

void loop() 
{
  
  if((leftc = getPing(9)) != 0) left = leftc;
  delay(15);
  if((frontc = getPing(10)) != 0) front = frontc; //get reading excluding bad readings of zero
  delay(15);                                      //delay for cross-talk and time for interrupts to trigger
  if((rightc = getPing(11)) != 0) right = rightc;
  delay(15);

  if(front < 3000 || left < 650 || right < 650) //something is close in front
  {
    drive.write(95);                  //make sure the robot isn't in reverse to ensure the brake triggers
    delay(25);
    drive.write(0);                             //activate brake
    delay(775);                                 //give time to stop
    if(left >= right) {                         //steer away from objects
      frontsteer.write(frontcenter+turn);
      backsteer.write(backcenter-turn);
    }
    else {
      frontsteer.write(frontcenter-turn);
      backsteer.write(backcenter+turn);
    }
    drive.write(95);                           //release brake
    delay(50);
    drive.write(revspeed);                     //start reverse
    starttime = millis();
    while(
    ((millis()-starttime<1000) 
      || front<5500 )
      && ((digitalRead(13)==1) 
      && (digitalRead(12)==1)))
    {
      front = getPing(5);
      delay(5);
    }

    drive.write(90);                          //stop
    frontsteer.write(frontcenter);            //turn to drive straight
    backsteer.write(backcenter);
    delay(1000);
    drive.write(fwdspeed);                    //start going forward again
  }

  if(left < 4000 && right > 4000)             //if left side is getting close
  {
    frontsteer.write(map(left, 4000, 650, frontcenter, (frontcenter+turn)));     //scale the servo from 
    //center towards turning 

    backsteer.write(map(left, 4000, 650, backcenter, (backcenter-turn)));//by the ping sensors reported
    //distance from the wall

  }
  else if(left > 4000 && right < 4000)       //if right side is getting close
  {  
    frontsteer.write(map(right, 4000, 650, frontcenter, (frontcenter-turn)));
    backsteer.write(map(right, 4000, 650, backcenter, (backcenter+turn)));
  }
  else if(left > 4000 && right > 4000)
  {
    frontsteer.write(frontcenter);           //everything is fine go straight ahead
    backsteer.write(backcenter);
    drive.write(fwdspeed);
  }

}