#include <Servo.

h>
#include <NewPing.h>

#define TRIG_PIN A4 // Pin A4 on the Arduino Sensor Shield

#define ECHO_PIN A5 // Pin A5 on the Arduino Sensor Shield
#define MAX_DISTANCE 200 // sets maximum useable sensor measuring distance to 200cm
#define MAX_SPEED 180 // sets speed of DC traction motors to 180/256 or about 70% of full speed - to keep power drain down.
#define MAX_SPEED_OFFSET 20 // this sets offset to allow for differences between the DC traction motors
#define COLL_DIST 10 // sets distance at which robot stops and reverses to 10cm
#define TURN_DIST COLL_DIST+25 // sets distance at which robot veers away from object (not reverse) to 25cm (10+15)

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); // sets up sensor library to use the correct pins to measure distance.

//Assign digital pins used to drive the motors

int ENA=5;//connected to Arduino's port 5(output pwm)

int IN1=2;//connected to Arduino's port 2
int IN2=3;//connected to Arduino's port 3
int ENB=6;//connected to Arduino's port 6(output pwm)
int IN3=4;//connected to Arduino's port 4
int IN4=7;//connected to Arduino's port 7

Servo myservo; // create servo object to control a servo

int pos = 0; // this sets up variables for use in the sketch (code)
int curDist = 0; //Current distance
int maxDist = 0; //Maximum distance from a sweep of pings
int maxPos = 0; //Angle at which maximum distance was detected.
String motorSet = "";
int speedSet = 0;

//-------------------------------------------- SETUP LOOP ----------------------------------------------------------------

void setup() {
//Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results
pinMode(ENA,OUTPUT);//output
pinMode(ENB,OUTPUT);
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
pinMode(IN3,OUTPUT);
pinMode(IN4,OUTPUT);
digitalWrite(ENA,LOW);
digitalWrite(ENB,LOW);//stop driving
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);//setting motorA's direction
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);//setting motorB's direction
myservo.attach(9); // attaches the servo on pin 9
myservo.write(90); // tells the servo to position at 90-degrees ie. facing forward.
delay(2000); // delay for two seconds
lookAround(); //identify furthest object and direction
setCourse(); //off we go.

}
//---------------------------------------------------------------------------------------------------------------------------
//---------------------------------------------MAIN LOOP --------------------------------------------------------------------
void loop() {
checkForward(); // check robot is moving forward
checkPath(); // set ultrasonic sensor to scan for any possible obstacles
}
//--------------------------------------------------------------------------------------------------------------------------------
void checkPath() {
myservo.write(144); // set servo to face left 54-degrees from forward
delay(100); // wait 100ms for servo to reach position
for(pos = 144; pos >= 36; pos-=18) // loop to sweep the servo (& sensor) from 144deg left to 18deg right at 18deg intervals.
{
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(60); // wait 60ms for servo to get to position
curDist = readPing(); // get the current distance to any object in front of sensor
//Serial.println (curDist);
if (curDist < COLL_DIST) { // if the current distance to object is less than the collision distance
checkCourse(); // run the checkCourse function
break; // jump out of this loop
}
if (curDist < TURN_DIST) { // if current distance is less than the turn distance
changePath(); // run the changePath function
}
}
}
//---------------------------------------------------------------------------------------------------------------------------------
void lookAround() { //have a look around and identify at what angle is the furthest ping. Only run when staionary.
maxDist = 0;
maxPos= 144;
myservo.write(144); // set servo to face left 54-degrees from forward
delay(100); // wait 100ms for servo to reach position
for(pos = 144; pos >= 36; pos-=18) // loop to sweep the servo (& sensor) from 144deg left to 18deg right at 18deg intervals.
{
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(60); // wait 60ms for servo to get to position
curDist = readPing(); // get the current distance to any object in front of sensor
//Serial.println (curDist);
if (curDist > maxDist) {maxDist = curDist; maxPos=pos;}
}
}
//----------------------------------------------------------------------------------------------------------------------------------
void setCourse() { // have a look around turn towards the furthest object and then move forward
lookAround ();
if (maxPos <= 54) {turnRight();} // turn right as furthest object was to the right
if (maxPos >54 && maxPos < 90) {veerRight();} // veer right as furthest object is forward and slightly right
if (maxPos >= 126) {turnLeft();} // turn left as furthest object was to the left
if (maxPos >= 90 && maxPos < 126) {veerLeft();} //veer left as furthest object is ahead or slightly to the Left
}
//----------------------------------------------------------------------------------------------------------------------------------
void checkCourse() { // we're about to hit something so stop, move back a bit , stop and work out best course
moveStop();
delay(100);
moveBackward();
delay(600); //move backwards for 600ms
moveStop();
setCourse();
}
//----------------------------------------------------------------------------------------------------------------------------------
void changePath() {
if (pos < 90) {veerLeft();} // if current pos of sensor is less than 90deg, then object is on the right hand side so veer left
if (pos >= 90) {veerRight();} // if current pos of sensor is >= than 90deg, then object is on the left hand side or ahead so veer right
}
//----------------------------------------------------------------------------------------------------------------------------------

int readPing() { // read the ultrasonic sensor distance

delay(70);
unsigned int uS = sonar.ping();
int cm = uS/US_ROUNDTRIP_CM;
return cm;
}
//----------------------------------------------------------------------------------------------------------------------------------
// Motor direction functions. MotorA is on the right and MotorB on the left.
//----------------------------------------------------------------------------------------------------------------------------------
void checkForward() { // make sure both motors going forward.
if (motorSet=="FORWARD") {
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);//setting motorA's direction FWD
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);//setting motorB's direction FWD
}
}
//----------------------------------------------------------------------------------------------------------------------------------
void checkBackward() { // make sure both motors going backward
if (motorSet=="BACKWARD") {

digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);//setting motorA's direction BACK
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);//setting motorB's direction BACK
}
}
//----------------------------------------------------------------------------------------------------------------------------------
void checkRight() { // Motor A BACK and Motor B FWD
if (motorSet=="RIGHT") {
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);//setting motorA's direction BACK
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);//setting motorB's direction FWD
}
}
//----------------------------------------------------------------------------------------------------------------------------------
void checkLeft() { // Motor A FWD and Motor B Back
if (motorSet=="LEFT") {

digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);//setting motorA's direction FWD
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);//setting motorB's direction BACK
}
}
//----------------------------------------------------------------------------------------------------------------------------------
//
// The above functions simply set the L298 IN1,2,3,&4 pins for the required Motor direction

//----------------------------------------------------------------------------------------------------------------------------------
void moveStop() { // stop the motors
digitalWrite(ENA,LOW);
digitalWrite(ENB,LOW);//stop driving
}
//-----------------------------------------------------------------------------------------------------------------------------------
void moveForward() {
motorSet = "FORWARD";
checkForward();
for (speedSet = 140; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up
{
analogWrite(ENA,speedSet);//start driving motorA
analogWrite(ENB,speedSet+MAX_SPEED_OFFSET);//start driving motorB
delay(5);
}
}
//----------------------------------------------------------------------------------------------------------------------------------
void moveBackward() {
motorSet = "BACKWARD";
checkBackward();
for (speedSet = 140; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up
{
analogWrite(ENA,speedSet);//start driving motorA
analogWrite(ENB,speedSet+MAX_SPEED_OFFSET);//start driving motorB
delay(5);
}
}
//----------------------------------------------------------------------------------------------------------------------------------
void turnRight() {
motorSet = "RIGHT";
checkRight();
analogWrite(ENA,140);//start driving motorA
analogWrite(ENB,140+MAX_SPEED_OFFSET);//start driving motorB
delay(400); // run motors this way for 400ms
moveForward ();
}
//-----------------------------------------------------------------------------------------------------------------------------------
void turnLeft() {
motorSet = "LEFT";
checkLeft();
analogWrite(ENA,140);//start driving motorA
analogWrite(ENB,140+MAX_SPEED_OFFSET);//start driving motorB
delay(400); // run motors this way for 400ms
moveForward();
}
//-----------------------------------------------------------------------------------------------------------------------------------
void veerRight() { // veering right
motorSet = "RIGHT";
checkRight();
analogWrite(ENA,140);//start driving motorA
analogWrite(ENB,140+MAX_SPEED_OFFSET);//start driving motorB
delay(300); // run motors this way for 300ms
 moveForward ();
}
//----------------------------------------------------------------------------------------------------------------------------------
void veerLeft() { // veering left
motorSet = "LEFT";
checkLeft();
analogWrite(ENA,140);//start driving motorA
analogWrite(ENB,140+MAX_SPEED_OFFSET);//start driving motorB
delay(300); // run motors this way for 300ms
moveForward();
}