Trang chủ —› Chia sẻ kiến thức —› Lập trình robot tránh vật cản – Chia sẻ code mẫu

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

19 Tháng Ba,2018 Đăng bởi: admin

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:// quay trái
motorControlNoSpeed(inR1, inR2, 1);
motorControlNoSpeed(inL1, inL2, 2);
break;
case 4:// quay 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);
}
}
}
}


Notice: WP_Query được gọi với một tham số đã bị loại bỏ kể từ phiên bản 3.1.0! Hãy sử dụng ignore_sticky_posts thay cho caller_get_posts (sẽ sớm bị loại bỏ) in /home/lapt0260/public_html/wp-includes/functions.php on line 4045

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 *