#include <NewPing.h>
#include <Servo.h>
#define MAX_DISTANCE 200
#define TRIG_PIN 4
#define ECHO_PIN 2
#define LEFT 0
#define CENTER 90
#define RIGHT 180
#define ENABLE1 3
#define INPUT1 9
#define INPUT2 8
#define ENABLE2 11
#define INPUT3 7
#define INPUT4 6
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
Servo ultrasonicServo;
float dangerThreshold = 40.0;
int currentPos = 0;
#define LEFT_FORWARD_RIGHT_FORWARD {HIGH, HIGH, LOW, HIGH, HIGH, LOW}
#define LEFT_OFF_RIGHT_FORWARD {LOW, HIGH, LOW, LOW, HIGH, LOW}
#define LEFT_FORWARD_RIGHT_OFF {HIGH, LOW, LOW, HIGH, LOW, LOW}
#define LEFT_REVERSE_RIGHT_FORWARD {HIGH, HIGH, HIGH, LOW, HIGH, LOW}
#define LEFT_FORWARD_RIGHT_REVERSE {HIGH, HIGH, LOW, HIGH, LOW, HIGH}
#define LEFT_REVERSE_RIGHT_REVERSE {HIGH, HIGH, HIGH, LOW, LOW, HIGH}
#define LEFT_OFF_RIGHT_OFF {HIGH, HIGH, LOW, LOW, LOW, LOW}
#define LEFT_FREEWHEEL_RIGHT_FREEWHEEL {LOW, LOW, LOW, LOW, LOW, LOW}
#define FORWARD LEFT_FORWARD_RIGHT_FORWARD
#define REVERSE LEFT_REVERSE_RIGHT_REVERSE
#define LEFT_TURN LEFT_OFF_RIGHT_FORWARD
#define RIGHT_TURN LEFT_FORWARD_RIGHT_OFF
#define ROTATE_LEFT LEFT_REVERSE_RIGHT_FORWARD
#define ROTATE_RIGHT LEFT_FORWARD_RIGHT_REVERSE
#define BRAKE LEFT_OFF_RIGHT_OFF
#define FREEWHEEL LEFT_FREEWHEEL_RIGHT_FREEWHEEL
#define SLOW 130;
#define MEDIUM 195;
#define FAST 255;
int throttle = MEDIUM;
void setup() {
pinMode(ENABLE1, OUTPUT);
pinMode(ENABLE2, OUTPUT);
pinMode(INPUT1, OUTPUT);
pinMode(INPUT2, OUTPUT);
pinMode(INPUT3, OUTPUT);
pinMode(INPUT4, OUTPUT);
ultrasonicServo.attach(5);
servo_position(CENTER);
}
void loop() {
float distanceForward = ping();
if (distanceForward > dangerThreshold)
{
drive_forward();
}
else
{
brake();
servo_position(LEFT);
float distanceLeft = ping();
servo_position(RIGHT);
float distanceRight = ping();
if (distanceLeft > distanceRight && distanceLeft > dangerThreshold) //if left is less obstructed
{
rotate_left();
}
else if (distanceRight > distanceLeft && distanceRight > dangerThreshold) //if right is less obstructed
{
rotate_right();
}
else
{
u_turn();
}
servo_position(CENTER);
}
}
void freewheel(){
const int driveControl[] = FREEWHEEL;
drive(driveControl);
delay(25);
void brake(){
const int driveControl[] = BRAKE;
drive(driveControl);
delay(25);
}
void drive_forward(){
const int driveControl[] = FORWARD;
drive(driveControl);
}
void drive_backward(){
const int driveControl[] = REVERSE;
drive(driveControl);
}
void turn_left(){
const int driveControl[] = LEFT_TURN;
drive(driveControl);
delay(600);
}
void turn_right(){
const int driveControl[] = RIGHT_TURN;
drive(driveControl);
delay(600);
}
void rotate_left(){
const int driveControl[] = ROTATE_LEFT;
drive(driveControl);
delay(300);
}
void rotate_right(){
const int driveControl[] = ROTATE_RIGHT;
drive(driveControl);
delay(300);
}
void u_turn(){
const int driveControl[] = ROTATE_RIGHT;
drive(driveControl);
delay(600);
}
void drive(const int settings[6]){
if (settings[0] == HIGH)
analogWrite(ENABLE1, throttle);
else
digitalWrite(ENABLE1, LOW);
if (settings[1] == HIGH)
analogWrite(ENABLE2, throttle);
else
digitalWrite(ENABLE2, LOW);
digitalWrite(INPUT1, settings[2]);
digitalWrite(INPUT2, settings[3]);
digitalWrite(INPUT3, settings[4]);
digitalWrite(INPUT4, settings[5]);
}
void servo_position(int newPos){
if (newPos > currentPos){
for(int pos=currentPos; pos < newPos; pos += 1)
{
ultrasonicServo.write(pos);
delay(15);
}
currentPos = newPos;
}
else if (newPos < currentPos){
for(int pos=currentPos; pos > newPos; pos -= 1)
{
ultrasonicServo.write(pos);
delay(15);
}
currentPos = newPos;
}
}
float ping(){
delay(50);
unsigned int seconds = sonar.ping();
if (seconds == 0)
return MAX_DISTANCE;
else
return seconds / US_ROUNDTRIP_CM;
}