#include <AFMotor.h>
#include <Servo.h>
#include <NewPing.h>
#define TRIG_PIN A4
#define ECHO_PIN A5
#define MAX_DISTANCE 200 // sets maximum usable sensor measuring distance to 200cm
#define MAX_SPEED 130 // sets speed of DC traction motors to 180/256 or about 45% of full speed - to get power drain down.
#define MAX_SPEED_OFFSET 10 // this sets offset to allow for differences between the two DC traction motors
#define COLL_DIST 20 // sets distance at which robot stops and reverses to 20cm
//#define TURN_DIST COLL_DIST+20 // sets distance at which robot veers away from object (not reverse) to 40cm (20+20)
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); // sets up sensor library to use the correct pins to measure distance.
AF_DCMotor motor1(1, MOTOR12_1KHZ); // create motor #1 using M1 output on Motor Drive Shield, set to 1kHz PWM frequency
AF_DCMotor motor2(2, MOTOR12_1KHZ); // create motor #2, using M2 output, set to 1kHz PWM frequency
Servo myservo; // create servo object to control a servo
int curDist = 0;
String motorSet = "";
int speedSet = 0;
int isMovingForward = 0;
void setup() {
//Serial.begin (9600);
myservo.attach(9); // attaches the servo on pin 9 (SERVO_2 on the Motor Drive Shield to the servo object
myservo.write(50); // tells the servo to position at 80-degrees ie. facing forward.
delay(1000);
myservo.write(90); // tells the servo to position at 90-degrees ie. facing forward.
delay(1000);
myservo.write(140); // tells the servo to position at 90-degrees ie. facing forward.
delay(1000);
curDist = readPing(); // Get Ping Distance.
delay(100); // Wait for 100ms.
curDist = readPing();
delay(100);
myservo.write(90); // Face Forward While Running
delay(1000);
}
void loop() {
OperateRobotCar();
}
//-------------------------------------------------------------------------------------------------------------------------------------
void OperateRobotCar() {
int curLeft = 0;
int curFront = 0;
int curRight = 0;
//Serial.println("Cur Distance - Coll Distance = " + String(curDist) + "Cm -" + String(COLL_DIST) + "Cm \n");
if (curDist <= COLL_DIST && isMovingForward == 1)
{
isMovingForward=0;
moveStop();
delay(300);
moveBackward();
delay(500);
moveStop();
delay(300);
curRight = lookRight();
delay(300);
curLeft = lookLeft();
delay(300);
if (curRight >= curLeft && curRight > COLL_DIST)
{
turnRight();
delay(300);
moveStop();
delay(300);
}
else if (curLeft > COLL_DIST)
{
turnLeft();
delay(300);
moveStop();
delay(300);
}
}
else if (curDist > COLL_DIST && isMovingForward==0)
{
isMovingForward = 1;
moveForward();
}
curDist = readPing();
}
int lookRight()
{
myservo.write(144);
delay(500);
int distance = readPing();
delay(100);
myservo.write(90);
return distance;
}
int lookLeft()
{
myservo.write(54);
delay(500);
int distance = readPing();
delay(100);
myservo.write(90);
return distance;
}
int readPing() { // read the ultrasonic sensor distance
delay(70);
unsigned int uS = sonar.ping();
int cm = uS / US_ROUNDTRIP_CM;
if (cm == 0)
{
cm = 250;
}
Serial.println(cm);
return cm;
}
void moveStop() {
if (motorSet == "STOP")
return;
motorSet = "STOP";
Serial.println("Motor Stop\n");
motor1.run(RELEASE); // stop the motors.
motor2.run(RELEASE);
}
void moveForward() {
if (motorSet == "FORWARD")
return;
motorSet = "FORWARD";
Serial.println("Move Forward\n");
motor1.run(FORWARD); // turn it on going forward
motor2.run(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
{
Serial.println("SpeedSet - MaxSpeOffset" + String(speedSet) + "-" + String(MAX_SPEED_OFFSET) + "\n");
motor1.setSpeed(speedSet + MAX_SPEED_OFFSET);
motor2.setSpeed(speedSet);
delay(5);
}
}
void moveBackward() {
if (motorSet == "BACKWARD")
return;
motorSet = "BACKWARD";
Serial.println("Move Backward\n");
motor1.run(BACKWARD);
motor2.run(BACKWARD);
for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 2)
{
motor1.setSpeed(speedSet + MAX_SPEED_OFFSET);
motor2.setSpeed(speedSet);
delay(5);
}
}
void turnRight() {
Serial.println("Move Right\n");
motorSet = "RIGHT";
motor1.run(FORWARD);
motor2.run(BACKWARD);
delay(400);
motorSet = "FORWARD";
motor1.run(FORWARD);
motor2.run(FORWARD);
}
void turnLeft() {
Serial.println("Move Left\n");
motorSet = "LEFT";
motor1.run(BACKWARD);
motor2.run(FORWARD);
delay(400);
motorSet = "FORWARD";
motor1.run(FORWARD); // turn it on going forward
motor2.run(FORWARD); // turn it on going forward
}
That's it. Circuit Connections and Program plays vital Role in completing this Project.