You are on page 1of 4

#include <Ultrasonic.

h>
#include <Servo.h>
#define
#define
#define
#define

MOTOR_1_FORWARD 8
MOTOR_1_BACK 9
MOTOR_2_FORWARD 10
MOTOR_2_BACK 11

#define TRIG 6
#define ECHO 7
#define HEAD_SERVO 5
#define OBSTRACLE_DISTANCE 30.0
#define TURN_DELAY 200
#define LOG false
Ultrasonic ultrasonic(TRIG, ECHO);
Servo headServo;
int servoAngle = 90;
int angleStep = 30;
float distance = 0;
void setup()
{
pinMode(MOTOR_1_FORWARD,
pinMode(MOTOR_1_BACK,
pinMode(MOTOR_2_FORWARD,
pinMode(MOTOR_2_BACK,

OUTPUT);
OUTPUT);
OUTPUT);
OUTPUT);

stopMove();
headServo.attach(HEAD_SERVO);
if(LOG) Serial.begin( 9600 );
}
void loop()
{
updateHeadAngle();
checkDistance();
moove();
delay(10);
}
void checkDistance()
{
distance = ultrasonic.Ranging(CM);
if(LOG) Serial.println(distance);
}
void moove()
{

if( distance > OBSTRACLE_DISTANCE )


{
if(LOG) Serial.println("FORWARD");
goForward();
delay(TURN_DELAY);
}
else
{
stopMove();
checkObstracle();
}
}
void checkObstracle()
{
int obsLeft = 0;
int obsRight = 0;
// Count the obstacles from left and right side
for(servoAngle = 0; servoAngle <= 180; servoAngle += 30)
{
headServo.write(servoAngle);
delay(TURN_DELAY);
checkDistance();
if(distance < OBSTRACLE_DISTANCE && servoAngle < 90) obsLeft++;
else if(distance < OBSTRACLE_DISTANCE) obsRight++;
}
if(LOG) Serial.print("TURN");
if(obsLeft && obsRight)
{
goBack();
delay(TURN_DELAY * 2);
if(obsLeft < obsRight) goLeft();
else goRight();
delay(TURN_DELAY);
}
else if(obsRight)
{
goLeft();
delay(TURN_DELAY);
}
else if(obsLeft)
{
goRight();
delay(TURN_DELAY);
}
else
{
goForward();

delay(TURN_DELAY);
}
}
void updateHeadAngle()
{
headServo.write(servoAngle);
servoAngle += angleStep;
if(servoAngle >= 150)
{
servoAngle = 150;
angleStep *= -1;
}
if(servoAngle <= 30)
{
servoAngle = 30;
angleStep *= -1;
}
}
void goForward()
{
digitalWrite(MOTOR_1_FORWARD,
digitalWrite(MOTOR_1_BACK,
digitalWrite(MOTOR_2_FORWARD,
digitalWrite(MOTOR_2_BACK,
}

HIGH);
LOW);
HIGH);
LOW);

void goBack()
{
digitalWrite(MOTOR_1_FORWARD,
digitalWrite(MOTOR_1_BACK,
digitalWrite(MOTOR_2_FORWARD,
digitalWrite(MOTOR_2_BACK,
}

LOW);
HIGH);
LOW);
HIGH);

void goLeft()
{
digitalWrite(MOTOR_1_FORWARD,
digitalWrite(MOTOR_1_BACK,
digitalWrite(MOTOR_2_FORWARD,
digitalWrite(MOTOR_2_BACK,
}

LOW);
HIGH);
HIGH);
LOW);

void goRight()
{
digitalWrite(MOTOR_1_FORWARD,
digitalWrite(MOTOR_1_BACK,
digitalWrite(MOTOR_2_FORWARD,
digitalWrite(MOTOR_2_BACK,
}

HIGH);
LOW);
LOW);
HIGH);

void stopMove()
{
digitalWrite(MOTOR_1_FORWARD, LOW);

digitalWrite(MOTOR_1_BACK,
LOW);
digitalWrite(MOTOR_2_FORWARD, LOW);
digitalWrite(MOTOR_2_BACK,
LOW);
}