Code chương trình biên dịch với Arduino:
#include
<NewPing.h>
#define
TRIGGER_PIN 5
#define
ECHO_PIN 4
#define
MAX_DISTANCE 100
#define sensor1 A0
// cảm biến 1
#define sensor2 A1
// cảm biến 2
#define sensor3 A2
// cảm biến 3
#define sensor4 A3
// cảm biến 4
#define sensor5 A4
// cảm biến 5
#define sw A5 //
công tắc chạm
#define LM1 11 // Động
cơ trái A
#define LM2 10 // Động
cơ trái B
#define RM1 9 // Động
cơ phải A
#define RM2 6 // Động
cơ phải B
NewPing
sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
int khoangcach;
void setup()
{
Serial.begin(9600);
pinMode(sensor1,INPUT);
pinMode(sensor2,INPUT);
pinMode(sensor3,INPUT);
pinMode(sensor4,INPUT);
pinMode(sensor5,INPUT);
pinMode(sw,INPUT);
pinMode(LM1,
OUTPUT);
pinMode(LM2,
OUTPUT);
pinMode(RM1,
OUTPUT);
pinMode(RM2,
OUTPUT);
}
void dung(){
digitalWrite(LM1,
LOW);
digitalWrite(LM2,
LOW);
digitalWrite(RM1,
LOW);
digitalWrite(RM2,
LOW);
}
void dithang(){
analogWrite(LM1,
30);
digitalWrite(LM2,
LOW);
digitalWrite(RM1,
LOW);
analogWrite(RM2,
35);
}
void tranhvatcan(){
dung();
delay(200);
analogWrite(LM1,
40);
analogWrite(LM2,
0);
analogWrite(RM1,
0);
analogWrite(RM2,
0);
while(digitalRead(sensor1)==1);
delay(130);
dithang();
delay(700);
analogWrite(LM1, 0);
analogWrite(LM2, 0);
analogWrite(RM1, 0);
analogWrite(RM2, 40);
delay(470);
dithang();
delay(1000);
analogWrite(LM1, 0);
analogWrite(LM2, 0);
analogWrite(RM1, 0);
analogWrite(RM2, 40);
delay(150);
dithang();
while((digitalRead(sensor3)==1)
);
dung();
delay(150);
analogWrite(LM1, 50);
analogWrite(LM2,
0);
analogWrite(RM1,
0);
analogWrite(RM2,
0);
while((digitalRead(sensor3)==0));
dung();
delay(500);
analogWrite(LM1,
50);
analogWrite(LM2,
0);
analogWrite(RM1,
0);
analogWrite(RM2,
0);
while((digitalRead(sensor1)==0));
}
void doline(){
if(digitalRead(sw)==0){
if((digitalRead(sensor1))
&& (digitalRead(sensor2)) && !(digitalRead(sensor3)) &&
(digitalRead(sensor4)) && (digitalRead(sensor5)) )// Di chuyển thẳng
{
analogWrite(LM1,
30);
digitalWrite(LM2,
LOW);
digitalWrite(RM1,
LOW);
analogWrite(RM2,
35);
}
else
if((digitalRead(sensor1)) && (digitalRead(sensor2)) &&
(digitalRead(sensor3)) && (digitalRead(sensor4)) &&
(digitalRead(sensor5)) )// Di chuyển thẳng
{
analogWrite(LM1,
30);
digitalWrite(LM2,
LOW);
digitalWrite(RM1,
LOW);
analogWrite(RM2,
35);
}
else
if((digitalRead(sensor1)) && !(digitalRead(sensor2)) &&
(digitalRead(sensor3)) && (digitalRead(sensor4)) &&
(digitalRead(sensor5)) ) // rẽ trái 1
{
analogWrite(LM1,
0);
analogWrite(LM2,
30);
analogWrite(RM1,
0);
analogWrite(RM2,
50);
while(!(digitalRead(sensor3)));
}
else
if(!(digitalRead(sensor1)) && (digitalRead(sensor2)) &&
(digitalRead(sensor3)) && (digitalRead(sensor4)) &&
(digitalRead(sensor5)) ) // rẽ trái 2
{
analogWrite(LM1,
0);
analogWrite(LM2,
50);
analogWrite(RM1,
0);
analogWrite(RM2,
80);
while(!(digitalRead(sensor3)));
}
else
if(!(digitalRead(sensor1)) && !(digitalRead(sensor2)) &&
(digitalRead(sensor3)) && (digitalRead(sensor4)) &&
(digitalRead(sensor5)) ) // rẽ trái gấp
{
analogWrite(LM1,
0);
analogWrite(LM2,
80);
analogWrite(RM1,
0);
analogWrite(RM2,
100);
while(!(digitalRead(sensor3)));
}
else
if((digitalRead(sensor1)) && (digitalRead(sensor2)) &&
(digitalRead(sensor3)) && !(digitalRead(sensor4)) &&
(digitalRead(sensor5)) ) // Rẻ phải 1
{
analogWrite(LM1,
50);
analogWrite(LM2,
0);
analogWrite(RM1,
30);
analogWrite(RM2,
0);
while(!(digitalRead(sensor3)));
}
else
if((digitalRead(sensor1)) && (digitalRead(sensor2)) &&
(digitalRead(sensor3)) && (digitalRead(sensor4)) &&
!(digitalRead(sensor5)) ) // Rẻ phải 2
{
analogWrite(LM1,
80);
analogWrite(LM2,
0);
analogWrite(RM1,
50);
analogWrite(RM2,
0);
while(!(digitalRead(sensor3)));
}
else
if((digitalRead(sensor1)) && (digitalRead(sensor2)) &&
(digitalRead(sensor3)) && !(digitalRead(sensor4)) &&
!(digitalRead(sensor5)) ) // Rẻ phải gap
{
analogWrite(LM1,
100);
analogWrite(LM2,
0);
analogWrite(RM1,
80);
analogWrite(RM2,
0);
while(!(digitalRead(sensor3)));
}
else
if(!(digitalRead(sensor1)) && !(digitalRead(sensor2)) &&
!(digitalRead(sensor3)) && !(digitalRead(sensor4)) &&
!(digitalRead(sensor5)) ) // Dừng khi 5 cảm biến phát hiện vạch đen
{
digitalWrite(LM1,
LOW);
digitalWrite(LM2,
LOW);
digitalWrite(RM1,
LOW);
digitalWrite(RM2,
LOW);
}}
else
{digitalWrite(LM1,
LOW);
digitalWrite(LM2,
LOW);
digitalWrite(RM1,
LOW);
digitalWrite(RM2,
LOW);
}
}
void loop()
{
khoangcach=
sonar.ping_cm();
if (khoangcach ==
0) {
khoangcach = 30;
}
if(khoangcach
<=17) {
tranhvatcan();
}
else doline();
}
( Nguyên lý + Code chuẩn) Link MediafireKhi nhấn nút "Tải Về Máy", bạn sẽ vào trang quảng cáo, vui lòng chờ 5 giây. Nút
hiện ra ở góc phải phía trên, nhấn vào để đến trang download------------------------------------------------------------------------------------
DV1 - NHẬN VẼ MẠCH - LẬP TRÌNH 8051/ARDUINO/STM LẤY TRONG NGÀY
BẤM XEM CÁCH ĐẶT HÀNG TẠI ĐÂY
-----------------------------------------------------------------------------------
DV2 - Mua linh kiện về làm mạch nhanh - Miễn phí Ship
DV3 - Kênh youtube chia sẻ kiến thức điện tử thực hành ngay
DV4 - Kênh cộng đồng Facebook Điện Tử ChipKool
CÔNG TY ECHIPKOOL - UY TÍN - CHẤT LƯỢNG - NHANH CHÓNG - BẢO MẬT
