// 12/29/08
// www.CuriousInventor.com/kits/robodino
// www.CuriousInventor.com/forums

// CC Attribution-Share Alike 3.0
// http://creativecommons.org/licenses/by-sa/3.0/

// no warranties whatsoever, whether express, implied, or statutory, including, but not limited to, 
// any warranty of merchantability or fitness for a particular purpose or any warranty that the contents
// of the item will be error-free. 


#include <SoftwareServo.h>
#include <Servo.h>

#define LEFT_CENTER 93
#define RIGHT_CENTER 95
#define SPEED 5

Servo servo_left;
Servo servo_right;
SoftwareServo servo_scanner;

int pingPin = 7;
int scanned_values[20];
int run = 1;

void setup()
{
 
  servo_scanner.attach(5); // we attach drive servos later to keep them from creeping 
  Serial.begin(9600);
  Serial.print("Ready");
}

void loop()
{
  static int value = 0;
  static char CurrentServo;
  static long duration;
  int closest; 

  if (run == 1) { 
   // scan horizon and store values in scanned_values
   scan_horizon(); 
   // figure out which direction has the closest value
   // 0 means it's all the way on the left, 9 middle, 18 full right
   closest = find_closest();
 
  // rotate to face nearest scanned value
  // positive values are clockwise
   turn((closest - 9));
 
   // advance for a bit
   drive_straight(5);
  }
 
 
   delay(200);
 
 // Serial command interface
 if ( Serial.available()) {
    char ch = Serial.read();
    switch(ch) {
      case 'g': // go
        run = 1;
        break;
      case 's': // stop
        run = 0;
        break;
      case 'A':        
        CurrentServo='A';     // center 93
        break;
      case 'B':        
        CurrentServo='B';     // center 95
        break;
      case 'C':      
        CurrentServo='C';     // 0 left to 180 full right
        break;   
      case '+':
        value = value +1;       
        break;
      case '-':
        value = value -1;
       
        break;      
      case '0' ... '9':
        value=(ch-'0')*20;       
        break;
    }
    if (CurrentServo=='A') {
      servo_left.write(value);
    }
    else if (CurrentServo=='B') {
      servo_right.write(value);
    }       
    else if (CurrentServo=='C') {
      servo_scanner.write(value);
    }
    Serial.print("current servo: ");
    Serial.println(CurrentServo);
    Serial.println(value);
 }
 
 SoftwareServo::refresh();
  
}





int find_closest() {
  int i;
  int m  = 1000;
  int index = 0;
   for (i=1;i<=17;i+=1) {
      if (scanned_values[i] < m) {
         index = i;
         m = scanned_values[i]; 
      }
   }
   Serial.print("closest: ");
   Serial.print(index);
   Serial.print(" ");
   Serial.println(scanned_values[index]);
   return index;   
}



void scan_horizon() {
  int i, j;
  // return to full left. send command and then give it time to move.
  servo_scanner.write(0);
  for (j=0;j<40;j++) {    
      SoftwareServo::refresh();
      delay(20);   
  }
  // take 19 distance readings
  for (i=0;i<=18;i+=1) {
    SoftwareServo::refresh();
    servo_scanner.write(i*10);
    
    millisSet(0);
    while(millis() <140)
      SoftwareServo::refresh();      
    delay(20);
    
    scanned_values[i] =  ping();
    delay(10);
  }
  
  // read out scanned values to serial port:
  Serial.println("new scan:");
  for (i=0;i<=18;i++) {
     Serial.print(scanned_values[i]);
    Serial.print(' '); 
  }
  Serial.println();
  
    
}

void drive_straight(int distance) {
   // our servos have slightly different centers, and are oriented oppositely
   attach_drive_servos();
   if (distance > 0) {
     servo_left.write(LEFT_CENTER - SPEED);
     servo_right.write(RIGHT_CENTER + SPEED);
   }
   else if (distance < 0) {
     servo_left.write(LEFT_CENTER + SPEED);
     servo_right.write(RIGHT_CENTER - SPEED);     
   }
   else {
     detach_drive_servos();     
   }
   delay(abs(distance)*100);
   // stop drive servos
   detach_drive_servos();
}

void turn(int distance) {
   // our servos have slightly different centers, and are oriented oppositely
   // we attach and detach because it was impossible to get a perfect "center" or "off" position
   attach_drive_servos();
   if (distance > 0) {
     servo_left.write(LEFT_CENTER - SPEED);
     servo_right.write(RIGHT_CENTER - SPEED);
   }
   else if (distance < 0) {
     servo_left.write(LEFT_CENTER + SPEED);
     servo_right.write(RIGHT_CENTER + SPEED);     
   }
   else {
     detach_drive_servos();
   }
   delay(abs(distance)*100);
   // stop drive servos
   detach_drive_servos();
}

long ping() {
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  
   // According to Parallax's datasheet for the PING))), there are
  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
  // second). 
  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
  pinMode(pingPin, INPUT);
  return pulseIn(pingPin, HIGH) / 72 / 2;
}

void attach_drive_servos() {
  servo_left.attach(10);
  servo_right.attach(9);
}

void detach_drive_servos() {
  servo_left.detach();
  servo_right.detach();
}