/////////////////////////////////////////////////
//              eGizmo OBSTASONIC BOT                //
//                                             //
// This programs a sumobot with built in line  //
// sensor to avoid lines and stay in a ring.   //
// It also has an ultrasonic sensor that       //
// tells the bot to ram the target but avoids  //
// the ring if detected.                       //
//                                             //
// Codes by:                                   //
// e-Gizmo Mechatronix Central                 //
// Taft, Manila, Philippines                   //
// http://www.egizmo.com                       //
// July 16, 2013                                //
/////////////////////////////////////////////////

#include <NewPing.h>
// Connections:

#define line1 5    // Line sensor
#define line2 6    // Line sensor
#define line3 7    // Line sensor
#define motor1 8   // Motor control HIGH = FWD, LOW = BWD
#define motor2 9   // Speed input
#define motor3 10  // Speed input
#define motor4 11  // Motor control HIGH = FWD, LOW = BWD
#define trigger 12 // US trigger pin
#define echo 4     // US echo pin 

int maxdistance = 200;

NewPing sonar(trigger,echo,maxdistance); // NewPing setup of pins and maximum distance.


int speed;
int ramspeed = 250;              // Ram speed
int safespeed = 150;              // Neutral speed
int stucktimer = 0;
int timer = 0;

int liner = 0;
int randomizer = 0;
int z = 0;
int y = 0;

float distance;                  // Distance measured by the ultrasonic sensor

void setup() {                
  Serial.begin(9600);

  pinMode(motor1, OUTPUT);       // Set all motor driver pins to output
  pinMode(motor2, OUTPUT);
  pinMode(motor3, OUTPUT);
  pinMode(motor4, OUTPUT);

  pinMode(line1, INPUT);         // All line sensor pins should be set as input
  pinMode(line2, INPUT);
  pinMode(line3, INPUT);
}

void loop()  {

  ultraread();

  if(distance > 15)
  {
    safeDrive();
    Serial.println("safedrive");
  }
    if((z == 0 && distance <= 15) && y == 0)
  {
    z = 1;
  }

  if(z == 1  && timer <= 10)
  {

    LEFT_TURN();
    timer++;
    stucktimer++;
    delay(40);

    if(timer == 10)
    {
      timer = 20;
      y = 1;
      z = 2;
    }
  }
 
  // If right is not the way, turn left
  if(distance <= 15 && y == 1)
  {
    y = 2;
  }
    
  if(y == 2 && timer >= 20)
  {
    RIGHT_TURN();
    timer++;
        stucktimer++;
    delay(40);
    
    if(timer == 30)
    {
      timer = 0;
      y = 0;
      z = 0;
    }
  }
  
  // If bot is stuck,
  if(stucktimer >= 100)
  {
    stucktimer = 0;
    ROTATE();
    delay(800);
  }
    



}

// PRESET FUNCTIONS:

void ultraread()
{
  delay(10);
  unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
  distance = sonar.convert_cm(uS); 
}

void safeDrive()
{    
  digitalWrite(motor1, HIGH);
  digitalWrite(motor4, HIGH);

  analogWrite(motor2, safespeed);    
  analogWrite(motor3, safespeed);    

}

void WIGGLE()      // One can use this instead of the rotate
{
  digitalWrite(motor1,LOW);
  digitalWrite(motor2,HIGH);
  digitalWrite(motor3,HIGH);
  digitalWrite(motor4,HIGH);
  delay(200);
  digitalWrite(motor1,HIGH);
  digitalWrite(motor2,HIGH);
  digitalWrite(motor3,HIGH);
  digitalWrite(motor4,LOW);
  delay(200);
  digitalWrite(motor1,LOW);
  digitalWrite(motor2,LOW);
  digitalWrite(motor3,LOW);
  digitalWrite(motor4,LOW);

}

void ROTATE()    // Default reaction after timer runs out
{
  digitalWrite(motor1,LOW);
  digitalWrite(motor4,HIGH);
  digitalWrite(motor2,HIGH);
  digitalWrite(motor3,HIGH);
}



void STOP()
{
  digitalWrite(motor1,HIGH);
  digitalWrite(motor4,HIGH);

  analogWrite(motor2,1);
  analogWrite(motor3,1);
  delay(10);
}

void CHARGE()
{
  digitalWrite(motor1,HIGH);
  analogWrite(motor2,ramspeed);
  analogWrite(motor3,ramspeed);
  digitalWrite(motor4,HIGH);
}



void LEFT_TURN()
{     
  digitalWrite(motor1,HIGH);
  digitalWrite(motor4,HIGH);

  analogWrite(motor2,10);
  analogWrite(motor3,250);
}

void RIGHT_TURN()
{
  digitalWrite(motor1,HIGH);
  digitalWrite(motor4,HIGH);

  analogWrite(motor2,250);
  analogWrite(motor3,10);
}