#include <NewPing.h>
#include <AFMotor.h>
#include <Servo.h>
// Define Sonar Sensor Pins.
#define TRIGGER_PIN 22 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 24 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
Servo eyes;
int pingnorm;
int pingleft;
int pingright;
int pingcenter;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
// Adafruit Motor Setup (Declares Motors)
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);
AF_DCMotor motor4(4);
// Define Data Points
int motorspeed = 235; // MAX SPEED IS 255
int safedist = 5; // In Inches
void setup(){
// Adafruit Motor Setup Code
motor1.setSpeed(motorspeed);
motor2.setSpeed(motorspeed);
motor3.setSpeed(motorspeed);
motor4.setSpeed(motorspeed);
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
// Servo For Ultrasonic Sensor
eyes.attach(9);
// Serial Communication
Serial.begin(9600);
// Boot Up Code
Serial.print("Sonar Bot Booting Up");
delay(1);
Serial.print(".");
delay(500);
Serial.print(".");
delay(500);
Serial.print(".");
delay(500);
Serial.print("\nBoot Up Successful!");
}
void loop(){
// Check In Center
eyes.write(20);
Serial.print("Robot Scanning Center");
delay(100);
Serial.print("\n.");
delay(100);
Serial.print("\n.");
delay(100);
pingnorm = sonar.ping_in();
Serial.print("\n.");
delay(100);
if (pingnorm > safedist){
Serial.print("No Object Detected\n");
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
}
else{
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
Serial.print("Object Detected!\n");
Serial.print("Scanning\n");
// Scan Left
eyes.write(0);
pingright = sonar.ping_in();
delay(1000);
// Scan Right
Serial.print(".");
eyes.write(40);
pingleft = sonar.ping_in();
delay(1000);
// Scan Center
Serial.print(".");
eyes.write(20);
pingcenter = sonar.ping_in();
delay(1000);
Serial.print(".\n");
// Decide For Best Direction
if (pingleft > pingright){
Serial.print("Object Detected On Right\n");
motor1.run(FORWARD);
motor4.run(FORWARD);
}
else if (pingright > pingleft){
Serial.print("Object Detected On Left\n");
motor2.run(FORWARD);
motor3.run(FORWARD);
}
}
}
