#include <AFMotor.
h> //add Adafruit Motor Shield library
#include <Servo.h> //add Servo Motor library
#include <NewPing.h> //add Ultrasonic sensor library
#define TRIG_PIN A0 // Pin A0 on the Motor Drive Shield soldered to the ultrasonic sensor
#define ECHO_PIN A1 // Pin A1 on the Motor Drive Shield soldered to the ultrasonic sensor
#define MAX_DISTANCE 300 // sets maximum useable sensor measuring distance to 300cm
#define MAX_SPEED 160 // sets speed of DC traction motors to 150/250 or about 70% of full speed - to get power drain
down.
#define MAX_SPEED_OFFSET 40 // this sets offset to allow for differences between the two DC traction motors
#define COLL_DIST 30 // sets distance at which robot stops and reverses to 30cm
#define TURN_DIST COLL_DIST+20 // sets distance at which robot veers away from object
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); // sets up sensor library to use the correct pins to measure
distance.
//Mert Arduino [Link]
AF_DCMotor leftMotor1(1, MOTOR12_1KHZ); // create motor #1 using M1 output on Motor Drive Shield, set to 1kHz
PWM frequency
AF_DCMotor leftMotor2(2, MOTOR12_1KHZ); // create motor #2, using M2 output, set to 1kHz PWM frequency
AF_DCMotor rightMotor1(3, MOTOR34_1KHZ);// create motor #3, using M3 output, set to 1kHz PWM frequency
AF_DCMotor rightMotor2(4, MOTOR34_1KHZ);// create motor #4, using M4 output, set to 1kHz PWM frequency
Servo myservo; // create servo object to control a servo
int leftDistance, rightDistance; //distances on either side
int curDist = 0;
String motorSet = "";
int speedSet = 0;
//-------------------------------------------- SETUP LOOP ----------------------------------------------------------------------------
void setup() { //Mert Arduino [Link]
[Link](10); // attaches the servo on pin 10 (SERVO_1 on the Motor Drive Shield to the servo object
[Link](90); // tells the servo to position at 90-degrees ie. facing forward.
delay(1000); // delay for one seconds
}
//------------------------------------------------------------------------------------------------------------------------------------
//---------------------------------------------MAIN LOOP ------------------------------------------------------------------------------
void loop() {
[Link](90); // move eyes forward
delay(90);
curDist = readPing(); // read distance
if (curDist < COLL_DIST) {changePath();} // if forward is blocked change direction
moveForward(); // move forward
delay(500);
}
//-------------------------------------------------------------------------------------------------------------------------------------
void changePath() { //Mert Arduino [Link]
moveStop(); // stop forward movement
[Link](36); // check distance to the right
delay(500);
rightDistance = readPing(); //set right distance
delay(500);
[Link](144); // check distace to the left
delay(700);
leftDistance = readPing(); //set left distance
delay(500);
[Link](90); //return to center
delay(100);
compareDistance();
}
void compareDistance() // find the longest distance
{
if (leftDistance>rightDistance) //if left is less obstructed
{
turnLeft();
}
else if (rightDistance>leftDistance) //if right is less obstructed
{
turnRight();
}
else //if they are equally obstructed
{
turnAround();
}
}
//-------------------------------------------------------------------------------------------------------------------------------------
int readPing() { // read the ultrasonic sensor distance
delay(70);
unsigned int uS = [Link]();
int cm = uS/US_ROUNDTRIP_CM;
return cm;
}
//-------------------------------------------------------------------------------------------------------------------------------------
void moveStop() {[Link](RELEASE); [Link](RELEASE); [Link](RELEASE);
[Link](RELEASE);} // stop the motors.
//-------------------------------------------------------------------------------------------------------------------------------------
void moveForward() { //Mert Arduino [Link]
motorSet = "FORWARD";
[Link](FORWARD); // turn it on going forward
[Link](FORWARD); // turn it on going forward
[Link](FORWARD); // turn it on going forward
[Link](FORWARD); // turn it on going forward
for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the
batteries too quickly
{
[Link](speedSet);
[Link](speedSet);
[Link](speedSet);
[Link](speedSet);
delay(5);
}
}
//-------------------------------------------------------------------------------------------------------------------------------------
void moveBackward() { //Mert Arduino [Link]
motorSet = "BACKWARD";
[Link](BACKWARD); // turn it on going backward
[Link](BACKWARD); // turn it on going backward
[Link](BACKWARD); // turn it on going backward
[Link](BACKWARD); // turn it on going backward
for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the
batteries too quickly
{
[Link](speedSet);
[Link](speedSet);
[Link](speedSet);
[Link](speedSet);
delay(5);
}
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnRight() { //Mert Arduino [Link]
motorSet = "RIGHT";
[Link](FORWARD); // turn motor 1 forward
[Link](FORWARD); // turn motor 2 forward
[Link](BACKWARD); // turn motor 3 backward
[Link](BACKWARD); // turn motor 4 backward
[Link](speedSet+MAX_SPEED_OFFSET);
[Link](speedSet+MAX_SPEED_OFFSET);
delay(1500); // run motors this way for 1500
motorSet = "FORWARD";
[Link](FORWARD); // set both motors back to forward
[Link](FORWARD);
[Link](FORWARD);
[Link](FORWARD);
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnLeft() { //Mert Arduino [Link]
motorSet = "LEFT";
[Link](BACKWARD); // turn motor 1 backward
[Link](BACKWARD); // turn motor 2 backward
[Link](speedSet+MAX_SPEED_OFFSET);
[Link](speedSet+MAX_SPEED_OFFSET);
[Link](FORWARD); // turn motor 3 forward
[Link](FORWARD); // turn motor 4 forward
delay(1500); // run motors this way for 1500
motorSet = "FORWARD";
[Link](FORWARD); // turn it on going forward
[Link](FORWARD); // turn it on going forward
[Link](FORWARD); // turn it on going forward
[Link](FORWARD); // turn it on going forward
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnAround() { //Mert Arduino [Link]
motorSet = "RIGHT";
[Link](FORWARD); // turn motor 1 forward
[Link](FORWARD); // turn motor 2 forward
[Link](BACKWARD); // turn motor 3 backward
[Link](BACKWARD); // turn motor 4 backward
[Link](speedSet+MAX_SPEED_OFFSET);
[Link](speedSet+MAX_SPEED_OFFSET);
delay(1700); // run motors this way for 1700
motorSet = "FORWARD";
[Link](FORWARD); // set both motors back to forward
[Link](FORWARD);
[Link](FORWARD);
[Link](FORWARD);
}