Scribd
Upload
9 views

Code 2

Uploaded by

svdh5c69nd
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
9 views

Code 2

Uploaded by

svdh5c69nd
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 3

#include <AFMotor.

h> // libraray for motor shield


#include <Servo.h> // libraray for servo motor
#include <NewPing.h> // libraray for ultrasonic sensor

const int Trig_Pin =A0;


const int Echo_Pin =A1;
const int Max_Speed =200;
const int Max_Dist = 250;

NewPing ultra_sonic(Trig_Pin, Echo_Pin, Max_Dist);

AF_DCMotor motor1(1,MOTOR12_1KHZ);
AF_DCMotor motor2(2,MOTOR12_1KHZ);
AF_DCMotor motor3(3,MOTOR34_1KHZ);
AF_DCMotor motor4(4,MOTOR34_1KHZ);

Servo myservo;

int distance=250;
int speedSet=0;
boolean goesForward=false;

void setup() {

myservo.attach(10);
myservo.write(0);
delay(2000);
distance=readDistance();
delay(100);
moveForward();
}

void loop() {
int dist_R = 0;
int dist_L = 0;
delay(100);

if(distance<=25){
moveStop();
delay(100);

moveBackward();
delay(200);

moveStop();
delay(100);

dist_R = right_Distance();
delay(100);
dist_L = left_Distance();
delay(100);

if(dist_R <= dist_L){


turnLeft();
moveStop();
}
else{
turnRight();
moveStop();
}
}
else{
moveForward();
}
distance = readDistance();

int left_Distance(){
myservo.write(170);
delay(500);
int dist = readDistance();
delay(100);
myservo.write(90);
return dist;
delay(100);
}

int right_Distance(){
myservo.write(10);
delay(500);
int dist = readDistance();
delay(100);
myservo.write(90);
return dist;
delay(100);
}

int readDistance(){
int cm = ultra_sonic.ping_cm();
if(cm<=0){ // this is done in case when sensor gives -ve
value which is absurd
cm=250;
}
return cm;
}

void moveStop() {
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
}

void moveForward() {

if(!goesForward)
{
goesForward=true;
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
for (speedSet = 0; speedSet < Max_Speed; speedSet +=2) // slowly bring the speed
up to avoid loading down the batteries too quickly
{
motor1.setSpeed(speedSet);
motor2.setSpeed(speedSet);
motor3.setSpeed(speedSet);
motor4.setSpeed(speedSet);
delay(5);
}
}
}
void moveBackward() {
goesForward=false;
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(BACKWARD);
motor4.run(BACKWARD);
for (speedSet = 0; speedSet < Max_Speed; speedSet +=2) // slowly bring the speed
up to avoid loading down the batteries too quickly
{
motor1.setSpeed(speedSet);
motor2.setSpeed(speedSet);
motor3.setSpeed(speedSet);
motor4.setSpeed(speedSet);
delay(5);
}
}

void turnRight() {
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(BACKWARD);
motor4.run(BACKWARD);
delay(500);
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
}

void turnLeft() {
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
delay(500);
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
}

You might also like