Lập trình robot tránh vật cản – Chia sẻ code mẫu

robot tránh vât cản

Robot tránh vật cản

Giới thiệu code mẫu cho robot tránh vật cản

Robot tránh vật cản là một mẫu robot cơ bản, dành cho người mới nhập môn trong gia đình những robot tự động. Do có nhiều bạn muốn tìm hiểu và xin code mẫu để tham khảo nên mình có bài viết này để chia sẻ code mẫu cho các bạn. Các video hướng dẫn trong bài viết mình xin up lên từ từ nhé

Robot tránh vật cản
Robot tránh vật cản

Video hoạt động của robot tránh vật cản

Code mẫu robot tránh vật cản

#include <Servo.h>
#define trig 3 //chân trig của HC-SR04
#define echo 4//chân echo của HC-SR04

Servo srf05; // create servo object to control a servo
#define inA1 6 //Định nghĩa chân in1 của động cơ A
#define inA2 7 //Định nghĩa chân in2 của động cơ A
#define inB1 8 //Định nghĩa chân in1 của động cơ B
#define inB2 9 //Định nghĩa chân in2 của động cơ B

//laptrinhdieukhien.com

void setup()
{
pinMode(inA1, OUTPUT);//Set chân in1 của dc A là output
pinMode(inA2, OUTPUT);//Set chân in2 của dc A là output
pinMode(inB1, OUTPUT);//Set chân in1 của dc B là output
pinMode(inB2, OUTPUT);//Set chân in2 của dc B là output
pinMode(trig,OUTPUT);//chân trig sẽ phát tín hiệu
pinMode(echo,INPUT);//chân echo sẽ nhận tín hiệu
Serial.begin(9600);
srf05.attach(5); // attaches the servo on pin 9 to the servo object
}

void loop()
{
objectAvoiderChooseWay ( inA1 , inA2, inB1, inB2, 50, 700);

digitalWrite(6, HIGH);
digitalWrite(7, LOW);
digitalWrite(8, HIGH);
digitalWrite(9,LOW);
}

int objectDistance_cm (byte angle)
{
srf05.write(angle);
delay(500);
unsigned long duration; //biến đo thời gian
int distance; //biến lưu khoảng cách /* phát xung từ chân trig */

digitalWrite(trig,0); //tắt chân trig
delayMicroseconds(2);
digitalWrite(trig,1); // phát xung từ chân trig
delayMicroseconds(5);// xung có độ dài 5 microSeconds
digitalWrite(trig,0);//tắt chân trig /* tính toán thời gian */

duration = pulseIn(echo,HIGH);//đo độ rộng xung HIGH ở chân echo.
distance = int(duration/2/29.412);//tính khoảng cách đến vật.
return distance;
}

void motorControlNoSpeed (byte in1,byte in2, byte direct)
{

switch (direct)
{
case 0:// Dừng không quay
digitalWrite(in1,LOW);
digitalWrite(in2,LOW);
break;
case 1:// Quay chiều thứ 1
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
break;
case 2:// Quay chiều thứ 2
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
break;
//default:
}
}

void robotMover (byte inR1, byte inR2, byte inL1, byte inL2, byte action)
{

switch (action)
{
case 0:// không di chuyển
motorControlNoSpeed(inR1, inR2, 0);
motorControlNoSpeed(inL1, inL2, 0);
break;
case 1://đi thẳng
motorControlNoSpeed(inR1, inR2, 1);
motorControlNoSpeed(inL1, inL2, 1);
break;
case 2:// lùi lại
motorControlNoSpeed(inR1, inR2, 2);
motorControlNoSpeed(inL1, inL2, 2);
break;
case 3:// quayquay
➤ (n) wharf usually built parallel to the shoreline
…by BeeDictionary.com
trái
motorControlNoSpeed(inR1, inR2, 1);
motorControlNoSpeed(inL1, inL2, 2);
break;
case 4:// quayquay
➤ (n) wharf usually built parallel to the shoreline
…by BeeDictionary.com
phải
motorControlNoSpeed(inR1, inR2, 2);
motorControlNoSpeed(inL1, inL2, 1);
break;
case 5:// rẽ trái
motorControlNoSpeed(inR1, inR2, 1);
motorControlNoSpeed(inL1, inL2, 0);
break;
case 6:// rẽ phải
motorControlNoSpeed(inR1, inR2, 0);
motorControlNoSpeed(inL1, inL2, 1);
break;
case 7:// rẽ lùi trái
motorControlNoSpeed(inR1, inR2, 2);
motorControlNoSpeed(inL1, inL2, 0);
break;
case 8:// rẽ lùi phải
motorControlNoSpeed(inR1, inR2, 0);
motorControlNoSpeed(inL1, inL2, 2);
break;
default:
action = 0;

}
}

void objectAvoiderChooseWay (byte inR1, byte inR2, byte inL1, byte inL2, byte allow_distance, int turn_back_time)
{

robotMover(inR1,inR2,inL1,inL2,1);
Serial.println(“Tiến”);
delay(10);
int front_distance = objectDistance_cm (90);
int left_distance;
int right_distance;
int max_distance;
if (front_distance > allow_distance)
{
robotMover(inR1,inR2,inL1,inL2,1);

delay(10);
}
if (front_distance <= allow_distance)
{
robotMover(inR1,inR2,inL1,inL2,2);

delay(300);
robotMover(inR1,inR2,inL1,inL2,0);
left_distance = objectDistance_cm (180); //đo khoảng cách bên trái

right_distance = objectDistance_cm (0); //đo khoảng cách bên phải

max_distance = max(left_distance,right_distance);// so sánh giá trị lớn nhất với khoảng cách bên phải (gán bằng cái lớn nhất)
if (max_distance==left_distance)
{
robotMover(inR1,inR2,inL1,inL2,3);

delay (turn_back_time/2); // Nếu bên trái là khoảng cách lớn nhất thì rẽ trái
}
else
{
if (max_distance==right_distance)
{
robotMover(inR1,inR2,inL1,inL2,4);// Nếu bên phải có khoảng cách lớn nhất thì rẽ phải

delay (turn_back_time/2);
}
}
}
}

 Save as PDF

Trả lời

Email của bạn sẽ không được hiển thị công khai. Các trường bắt buộc được đánh dấu *