/* 
Following code by EricWillaim
All build information, code and forum posts/discussion can be found at:
http://mkme.org

Dedicated build page:
http://www.mkme.org/index.php/arduino-sainsmart-4wd-robot/

This is my attempt at programming the Sainsmart 4WD robot
So far it now checks for a new direction periodically and performs
backup and turn functions as in reaction to the environment it is presented.
It is by far not optimized code- but works for now :)
I will update this periodically as I get time to learn new methods and improve the code. 
Please check out my Youtube videos here and consider a thumbs up if this helped you!
Youtube : http://www.youtube.com/user/Shadow5549

Some portions of this code adapted from:
http://www.duino-robotics.com/

V9 Dec 12 2013:
Added a forward distance check after turns- will continue to turn if distance still too small
Minor Tweaks but still using delay functions

V10 May 2014
Added support for Roam mode- serial commands will toggle roam versus individual driving commands
*/

#include <Servo.h> //servo library
#include "pitches.h"// used for the speaker output
#define SPEAKER 4// Speaker Pin

Servo headservo;
const int EchoPin = 2; // Ultrasonic signal input
const int TrigPin = 3; // Ultrasonic signal output
const int leftmotorpin1 = 8; //signal output of Dc motor driven plate
const int leftmotorpin2 = 9;
const int rightmotorpin1 = 6;
const int rightmotorpin2 = 7;
const int HeadServopin = 5; // signal input of headservo
const int maxStart = 800; //run dec time- no clue what this is
unsigned long time; //used for upcoming improvements (time used instead of loops)
unsigned long time1; //used for upcoming improvements (time used instead of loops)
int add= 0; //used for nodanger loop count
int add1= 0;  //used for nodanger loop count
int roam = 1;
int currDist = 5000; // distance
boolean running = false;// This is from old code examples

void setup() {
  //initialize beeps
  tone(SPEAKER, NOTE_C7, 100);
  delay(500);
  tone(SPEAKER, NOTE_C6, 100);
  tone(SPEAKER, NOTE_C7, 100);
  delay(500);
  tone(SPEAKER, NOTE_C6, 100);
  tone(SPEAKER, NOTE_C7, 100);
  delay(500);
  tone(SPEAKER, NOTE_C6, 100);
  //End Initialize Beeps
  
Serial.begin(9600); // Enables Serial monitor for debugging purposes
Serial.println("Serial Data Initiated!"); // Test the Serial communication
pinMode(EchoPin, INPUT);//signal input port

//signal output port
for (int pinindex = 3; pinindex < 10; pinindex++) {
pinMode(pinindex, OUTPUT); // set pins 3 to 10 as outputs
}

// headservo interface
headservo.attach(HeadServopin);
//start buffer movable head
headservo.write(160);
delay(1000);
headservo.write(20);
delay(1000);
headservo.write(90);
delay(1000);
return;
}
void loop() 

  {
  if (Serial.available() > 0) 
  {
    int data = Serial.read();	//read serial input commands
    switch(data)
    {
    case 'w' : 
      Serial.println("Rolling!");
      moveForward(); 
      break;
    case 'x' : 
      backup(); 
      buzz(); 
      break;
    case 'a' : 
      body_lturn(); 
      buzz(); 
      break;
    case 'd' : 
      body_rturn(); 
      buzz(); 
      break;
    case 's' : 
      totalhalt(); 
      break;
    case 'r' : 
      toggleRoam(); 
      buzz(); 
      break;
    
      }
  }
  
  if(roam == 0){ //just listen to serial commands and wait
   // Do something else if you like
  }
  else if(roam == 1){  //If roam active- drive autonymously
  
  
  
//time = millis(); // Sets "time" to current system time count
currDist = MeasuringDistance(); //measure front distance
Serial.print("Current Forward Distance: ");
//Serial.println(currDist);
if(currDist > 35) {
  add = (add1++);// Start adding up the loop count done in nodanger
nodanger();
Serial.println("Nodanger: ");
}
else if(currDist < 35){
  //add=0;
  Serial.println("Forward Blocked- Decide Which Way");
  backup();
  whichway();
  
}
  }
  
  }
//measure distance, unit “cm”
long MeasuringDistance() {
long duration;
//pinMode(TrigPin, OUTPUT);
digitalWrite(TrigPin, LOW);
delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);
delayMicroseconds(5);
digitalWrite(TrigPin, LOW);
//pinMode(EchoPin, INPUT);
duration = pulseIn(EchoPin, HIGH);
return duration / 29 / 2;
}
  

// forward
void moveForward() {
analogWrite(leftmotorpin1, 0);//Changed these to analog write for slower
analogWrite(leftmotorpin2, 120);
analogWrite(rightmotorpin1, 0);
analogWrite(rightmotorpin2, 120);
}

void toggleRoam(){
if(roam == 0){
   roam = 1;
   Serial.println("Activated Roam Mode");
  }
  else{
    roam = 0;
    totalhalt();
    Serial.println("De-activated Roam Mode");
  }
}

void nodanger() {
running = true;// Do I need these?
analogWrite(leftmotorpin1, 0);//Changed these to analog write for slower
analogWrite(leftmotorpin2, 120);
analogWrite(rightmotorpin1, 0);
analogWrite(rightmotorpin2, 120);
if (add1 > 38 ) whichway(); // Robot will stop and check direction every X loops through nodanger then resets in totalhalt (40 is good)
return;
}

//backward
void backup() {
add1=0; // resets the counter for the nodanger loops
running = true;//Do I need these?
digitalWrite(leftmotorpin1, HIGH);
digitalWrite(leftmotorpin2, LOW);
digitalWrite(rightmotorpin1, HIGH);
digitalWrite(rightmotorpin2, LOW);
Serial.println("Backing Up");
fart();
}

//choose which way to turn
void whichway() {
running = true;//Do I need these?
totalhalt();
headservo.write(160);
delay(900);
int lDist = MeasuringDistance(); // check left distance
Serial.println("checking left");
Serial.println(lDist);
headservo.write(20); // turn the servo right
delay(900);
int rDist = MeasuringDistance(); // check right distance
Serial.println("checking right");
Serial.println(rDist);
//totalhalt(); //  Do I need this???  Used to be used to centre the servo
if(lDist < rDist) {
Serial.println("Decided Right Is Best");
buzz();// Make him talk
body_rturn();
totalhalt();
currDist = MeasuringDistance(); //measure front distance
if(currDist < 45) body_rturn();  //if front distance still too small- turn again
}
else{
Serial.println("Decided Left Is Best");
  buzz();// Make him talk
  body_lturn();
  totalhalt();
  currDist = MeasuringDistance(); //measure front distance
  if(currDist < 45) body_lturn();// if front distance still too small- turn again
   }
return;
} 

void totalhalt() {
digitalWrite(leftmotorpin1, HIGH);
digitalWrite(leftmotorpin2, HIGH);
digitalWrite(rightmotorpin1, HIGH);
digitalWrite(rightmotorpin2, HIGH);
Serial.println("Totalhalt!");
headservo.write(90); // set servo to face forward
delay(250);
running = false;//Do I need these?
add1=0; // resets the counter for the nodanger loops
return;
}

//turn left
void body_lturn() {
running = true;
analogWrite(leftmotorpin1, 0);
analogWrite(leftmotorpin2, 220);
analogWrite(rightmotorpin1, 220);
analogWrite(rightmotorpin2, 0);
Serial.println("Turning Left");
delay(400);
}

//turn right
void body_rturn() {
running = true;
analogWrite(leftmotorpin1, 220);
analogWrite(leftmotorpin2, 0);
analogWrite(rightmotorpin1, 0);
analogWrite(rightmotorpin2, 220);
Serial.println("Turning Right");
delay(400);
}

void buzz(){
tone(SPEAKER, NOTE_C7, 100);
delay(50);
tone(SPEAKER, NOTE_C6, 100);
}

void fart(){
 tone(SPEAKER, NOTE_C2, 200);
 delay(50);
 tone(SPEAKER, NOTE_C1, 400); 
 delay(100);
 noTone(SPEAKER);
}