Scribd
Upload
15 views7 pages

Message

The document contains Arduino code for a robot that uses ultrasonic sensors to measure distances on the left, middle, and right sides. Based on these measurements, the robot can move forward, backward, turn, or stop depending on the distance detected. The code includes functions for setting up the sensors and controlling the motors to navigate obstacles.

Uploaded by

quangminh160505
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
15 views7 pages

Message

The document contains Arduino code for a robot that uses ultrasonic sensors to measure distances on the left, middle, and right sides. Based on these measurements, the robot can move forward, backward, turn, or stop depending on the distance detected. The code includes functions for setting up the sensors and controlling the motors to navigate obstacles.

Uploaded by

quangminh160505
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

int trig1 = A3;

int echo1 = A2; // middle


int trig2 = A5;
int echo2 = A4;
int trig3 = A0;
int echo3 = A1;

int in1 = 3;
int in2 = 4;
int in3 = 5;
int in4 = 6;
int ENA = 10;
int ENB = 11;
int LeftSpeed = 75;
int RightSpeed = 100;
long leftDistance = 0,middleDistance = 0,rightDistance = 0;
long pingTime,distance;
float speedSound = 0.0343;
int DIS = 5;

long leftMeasurement()
{
digitalWrite(trig1,LOW);
delayMicroseconds(2);
digitalWrite(trig1,HIGH);
delayMicroseconds(2);
digitalWrite(trig1,LOW);
pingTime = pulseIn(echo1,HIGH);
distance = (pingTime/2)*speedSound;
return(distance);
}

long middleMeasurement()
{
digitalWrite(trig2,LOW);
delayMicroseconds(2);
digitalWrite(trig2,HIGH);
delayMicroseconds(2);
digitalWrite(trig2,LOW);
pingTime = pulseIn(echo2,HIGH);
distance = (pingTime/2)*speedSound;
return(distance);
}

long rightMeasurement()
{
digitalWrite(trig3,LOW);
delayMicroseconds(2);
digitalWrite(trig3,HIGH);
delayMicroseconds(2);
digitalWrite(trig3,LOW);
pingTime = pulseIn(echo3,HIGH);
distance = (pingTime/2)*speedSound;
return(distance);
}

void setup()
{
[Link](9600);
pinMode(trig1,OUTPUT);
pinMode(trig2,OUTPUT);
pinMode(trig3,OUTPUT);
pinMode(echo1,INPUT);
pinMode(echo2,INPUT);
pinMode(echo3,INPUT);
pinMode(in1,OUTPUT);
pinMode(in2,OUTPUT);
pinMode(in3,OUTPUT);
pinMode(in4,OUTPUT);
pinMode(ENA,INPUT);
pinMode(ENB,INPUT);
moveStop();
}

void loop()
{
leftDistance = leftMeasurement();
delay(10);
middleDistance = middleMeasurement();
delay(10);
rightDistance = rightMeasurement();
delay(10);
[Link]("leftDistance = ");
[Link](leftDistance);
[Link]("cm /");
[Link]("middleDistance = ");
[Link](middleDistance);
[Link]("cm /");
[Link]("rightDistance = ");
[Link](rightDistance);
[Link]("cm");

if (middleDistance > DIS && leftDistance >= DIS ) {


moveForward(); //di thang neu khoang cach lon hon 10
delay(1000);
} else if (leftDistance < DIS && middleDistance > DIS ) {
moveStop();
moveBackward();
delay(1000);
turnRight();
delay(1000);
moveForward();
delay(1000);

} else if (middleDistance < DIS && leftDistance < DIS && rightDistance < DIS) {
//neu khoang canh middle nho hon 10 se co cac truong hop sau
moveStop();
delay(1000);
moveBackward();
delay(15);
turnRight();
delay(100);
moveForward();
delay(100);
}
}
void moveForward()
{
[Link]("Move Forward");
analogWrite(ENA,LeftSpeed);
analogWrite(ENB,RightSpeed);
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
delay(100);
analogWrite(ENA,LeftSpeed);
analogWrite(ENB,RightSpeed);
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);

void moveBackward()
{
[Link]("Move Backward");
analogWrite(ENA,LeftSpeed);
analogWrite(ENB,RightSpeed);
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
delay(10);

void turnRight()
{
[Link]("Turn Right");
analogWrite(ENA,LeftSpeed);
analogWrite(ENB,RightSpeed);
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
delay(100);
analogWrite(ENA,LeftSpeed);
analogWrite(ENB,RightSpeed);
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
// digitalWrite(in1, HIGH);
// digitalWrite(in2, LOW);
// digitalWrite(in3, LOW);
// digitalWrite(in4, HIGH);
}

void turnLeft()
{
[Link]("Turn Left");
analogWrite(ENA,LeftSpeed);
analogWrite(ENB,RightSpeed);
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
delay(100);
analogWrite(ENA,LeftSpeed);
analogWrite(ENB,RightSpeed);
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
}

void moveStop()
{
[Link]("Move Stop");
analogWrite(ENA,LOW);
analogWrite(ENB,LOW);
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
}

// // int motor_lA = 4;
// // int motor_lB = 7;
// // int motor_rA = 6;
// // int motor_rB = 5;

// int motor_lA = 4;
// int motor_lB = 3;
// int motor_rA = 10;
// int motor_rB = 9;

// int motor_enableA = 5;
// int motor_enableB = 11;

// int trigger_left = A1;


// int echo_left = A0;

// int trigger_front = A2;


// int echo_front = A3;

// int trigger_right = A4;


// int echo_right = A5;

// #define DIS 20

// void setup() {
// [Link](9600);
// // put your setup code here, to run once:
// pinMode(motor_lA,OUTPUT); //left motors forward
// pinMode(motor_lB,OUTPUT); //left motors reverse
// pinMode(motor_enableA, OUTPUT);

// pinMode(motor_rA,OUTPUT); //right motors forward


// pinMode(motor_rB,OUTPUT); //rignt motors reverse
// pinMode(motor_enableB, OUTPUT);

// pinMode(trigger_front,OUTPUT);
// pinMode(echo_front,INPUT);

// pinMode(trigger_left,OUTPUT);
// pinMode(echo_left,INPUT);

// pinMode(trigger_right,OUTPUT);
// pinMode(echo_right,INPUT);

// analogWrite(motor_enableA,90);
// analogWrite(motor_enableB, 90);

// }

// void loop() {
// // put your main code here, to run repeatedly:

// long duration_front, distance_front, duration_left, distance_left,


duration_right, distance_right;

// //Calculating distance

// digitalWrite(trigger_front, LOW);
// delayMicroseconds(2);
// digitalWrite(trigger_front, HIGH);
// delayMicroseconds(5);
// digitalWrite(trigger_front, LOW);
// duration_front = pulseIn(echo_front, HIGH);
// distance_front= duration_front*0.034/2;

// digitalWrite(trigger_left, LOW);
// delayMicroseconds(2);
// digitalWrite(trigger_left, HIGH);
// delayMicroseconds(5);
// digitalWrite(trigger_left, LOW);
// duration_left = pulseIn(echo_left, HIGH);
// distance_left= duration_left*0.034/2;

// digitalWrite(trigger_right, LOW);
// delayMicroseconds(2);
// digitalWrite(trigger_right, HIGH);
// delayMicroseconds(5);
// digitalWrite(trigger_right, LOW);
// duration_right = pulseIn(echo_right, HIGH);
// distance_right= duration_right*0.034/2;
// [Link]("front = ");
// [Link](distance_front);
// [Link]("Left = ");
// [Link](distance_left);
// [Link]("Right = ");
// [Link](distance_right);
// delay(50);
// if (distance_front >=8){
// forward();
// delay(500);
// if(distance_left>0 && distance_left <15){
// forward();
// delay(500);
// }
// if(distance_left>=15){
// Stop();
// left();
// delay(50);
// forward();
// }
// }

// if(distance_left <=20 && distance_right >20 && distance_front <=8) {


// Stop();
// delay(500);
// right();
// delay(800);
// forward();
// }

// if(distance_right<=20 &&distance_left<=20 && distance_front<=8) {


// Stop();
// delay(500);
// right();
// delay(1200);
// forward();
// }

// if(distance_left >20 && distance_right>20 && distance_front <=8) {


// Stop();
// delay(500);
// left();
// delay(800);
// forward();
// }

// if(distance_right <=20 && distance_left>20 && distance_front <=8) {


// Stop();
// delay(500);
// right();
// delay(1200);
// forward();
// }

// }

// void forward()
// {
// [Link]("Move Forward");
// digitalWrite(motor_lA,1);
// digitalWrite(motor_lB,0);
// digitalWrite(motor_rA,0);
// digitalWrite(motor_rB,1);
// delay(1000);
// }

// void backward()
// {
// [Link]("Move Backward");
// digitalWrite(motor_lA, LOW);
// digitalWrite(motor_lB, HIGH);
// digitalWrite(motor_rA, LOW);
// digitalWrite(motor_rB, HIGH);

// }

// void right(){
// [Link]("Move Right");
// digitalWrite(motor_lA,1);
// digitalWrite(motor_lB,0);
// digitalWrite(motor_rA,0);
// digitalWrite(motor_rB,1);

// }

// void left(){
// [Link]("Move Left");
// digitalWrite(motor_lA,0);
// digitalWrite(motor_lB,1);
// digitalWrite(motor_rA,1);
// digitalWrite(motor_rB,0);

// }

// void Stop(){
// [Link]("Stop");
// digitalWrite(motor_lA,0);
// digitalWrite(motor_lB,0);
// digitalWrite(motor_rA,0);
// digitalWrite(motor_rB,0);
// delay(100);
// }

You might also like