Nội dung chính
- Sơ đồ đấu nối phần cứng
- Các khai báo cần thiết
- Lập trình dò đường
- Lập trình đếm vạch
- Code tổng hợp sử dụng các hàm dò line
- Mô phỏng code trên Proteus
- Hướng dẫn sử dụng mô phỏng:
- Link dowload mã nguồn trong Project
Sơ đồ đấu nối phần cứng
Các khai báo cần thiết
C
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
//==========khai bao bien cho ham do duong=========
#define cb0 bit_is_set[PINF,0]// định nghĩa cb0 là check PINF0 có bằng 1 ko?
#define cb1 bit_is_set[PINF,1]
#define cb2 bit_is_set[PINF,2]
#define cb3 bit_is_set[PINF,3]
#define cb4 bit_is_set[PINF,4]
#define cb5 bit_is_set[PINF,5]
#define cb6 bit_is_set[PINF,6]
#define cb7 bit_is_set[PINF,7]
#define cb PINF// giá trị của cả PORTF được gán vào biến cb
// Khai bao cho do duong
unsigned char so_line// dùng cho đếm số vạch sẽ đi qua
unsigned char val_pwm,val_pwm_t,nho_line,pwm_speed;
unsigned char tam_pwm,bit_vung,bit_nhanh_cham,pwm_80,pwm_90,pwm_70,pwm_60,pwm_50,pwm_30,pwm_20;
un char L[10],R[10];// mảng vận tốc cho việc lái xe dò đường
Lập trình dò đường
Bảng dưới đây minh họa các trường hợp có thể xảy ra khi robot chạy theo đường line. Theo thiết kế mắt cảm biến, thì tại một thời điểm chỉ có thể tối đa 2 mắt cảm biến đang trên vạch trắng [vì vậy chỉ cần xét tối đa 3 mắt cảm biến tại một thời điểm].
Kí hiệu X là mắt cảm biến đang trên vạch trắng. Khi đó nếu ta gán các mắt cảm biến theo thứ tự Trái-> Phải tương ứng với PORTF thì các chân trên PORTF sẽ sáng tương ứng với vị trí X. Đây chính là cơ sở để chúng ta viết hàm dò đường.
Hàm dò line
Chúng ta sẽ dụng câu điều kiện if..else if cho hàm dò đường.
C
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
void do_lai[void]// cb7 ben phai , cb0 ben trai
{
if[cb==0 || [!cb3&&!cb4]] {tien_phai[R[0]];tien_trai[L[0]]; nho_line=4;}
// Chỉ cần sử dụng các mắt cảm biến nằm trong các trường hợp đã nêu trong
// bảng, các mắt cảm biến khác mặc định coi là tắt
// câu lệnh trên xét 2 trường hợp để cho robot đi với vận tốc max, đó là đi qua //vạch ngang [ diễn ra trong khoảng thời gian rất ngắn], và trường hợp 2 mắt //cảm biến chính giữa sáng.
// Ta đặt một biến nho_line để cho việc xử lý các hàm tiếp theo nếu xe lệch
else if [!cb3 && cb2 && cb4] {tien_phai[R[0]];tien_trai[L[1]];nho_line=4;}
// Các trường hợp khác làm tương tự như trên
else if [!cb4 && cb3 && cb5]
{tien_phai[R[1]];tien_trai[L[0]];nho_line=4;} //****X***
else if [!cb3 && !cb2]
{tien_phai[R[0]];tien_trai[L[2]];nho_line=0;}//**XX****
else if [!cb4 && !cb5]
{tien_phai[R[2]];tien_trai[L[0]];nho_line=0;}//****XX**
else if[!cb2 && cb1 && cb3]
{tien_phai[R[0]];tien_trai[L[3]];nho_line=0;}//**X*****
else if [!cb5 && cb4 &&cb6]
{tien_phai[R[3]];tien_trai[L[0]];nho_line=0;}//*****X**
else if [!cb2 && !cb1 ]
{tien_phai[R[0]];tien_trai[L[4]];nho_line=0;}//*XX*****
else if [!cb5 && !cb6 ]
{tien_phai[R[4]];tien_trai[L[0]];nho_line=0;}//*****XX*
else if[!cb1 && cb0 && cb2]
{tien_phai[R[0]];tien_trai[L[5]];nho_line=0;}//*X******
else if [!cb6 && cb7 && cb5]
{tien_phai[R[5]];tien_trai[L[0]];nho_line=0;}//******X*
else if [!cb1 && !cb0 ]
{tien_phai[R[0]];tien_trai[L[6]];nho_line=1;}//******XX
// đánh dấu là xe đã lệch hoàn toàn sang trái
else if [!cb6 && !cb7 ]
{tien_phai[R[6]];tien_trai[L[0]];nho_line=7;}//XX******
// đánh dấu là xe đã lệch hoàn toàn sang phải
else if[!cb0 && cb1]
{tien_phai[R[0]];tien_trai[L[6]];nho_line=1;}//*******X
else if [!cb7 && cb6]
{tien_phai[R[6]];tien_trai[L[0]];nho_line=7;}//X*******
else
{
if[nho_line==4] {tien_phai[20];tien_trai[20];}
else if[nho_line==7] {tien_phai[0];tien_trai[10];} // tự tìm line khi xe ra ngoài //hoàn toàn vạch line
else if[nho_line==1] {tien_phai[10];tien_trai[0];}
}
}
// hàm nạp vận tốc , thay đổi các tham sốnày vất vả nhất, cần điều chỉnh cho phù hợp đến khi nào
// robot chạy ổn định
void speed60[]
{
L[0]=56;L[1]=53;L[2]=53;L[3]=53;L[4]=53;L[5]=53;L[6]=53;
R[0]=60;R[1]=54;R[2]=54;R[3]=54;R[4]=54;R[5]=54;R[6]=54;
}
Lập trình đếm vạch
Đếm vạch có nghĩa là kiểm tra xem Robot đã đi qua bao nhiêu vạch ngang trên sân thi đấu. Thông thường chúng ta sẽ đếm số cảm biến sáng cùng nhau trong một khoảng thời gian, nếu có số cảm biến cùng sáng >4 thì ta kết luận là robot vừa đi qua được 1 vạch ngang.
C
1
2
3
4
5
6
7
8
9
10
11
12
13
// Hàm đếm số cảm biến sáng
un char dem_cb[]
{ un char i=0;
if[!cb0] i++; if[!cb1]i++; if[!cb2] i++; if[!cb3] i++;
if[!cb4] i++;if[!cb5] i++;if[!cb6] i++;if[!cb7] i++;
return i;
}
//=số cảm biến sáng > 4 là đếm 1 vạch
void dem_vach_ko_encoder[]
{
if[dem_cb[]>4]
{so_line++; }
}
Code tổng hợp sử dụng các hàm dò line
C
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
#include
#include
#include
#include
#include
#include
#include
#define gia_tri_start TCNT0
// khai bao dong co
#define Banh_trai OCR1AL//PWM 4 onboard
#define Banh_phai OCR1BL//PWM 5 onboard
#define Banh_1 OCR1CL//PWM 6 onboard
#define tay_xoay_tr OCR3AL//PWM 1 onboard
#define tay_xoay_ph OCR3BL//PWM 2 onboard
#define pha_a1 bit_is_clear[PINE,7]
#define pha_b1 bit_is_clear[PINE,6]
// KHAI BAO NUT NHAN PHUONG AN
#define cong_tac1 bit_is_clear[PING,0] //0
#define cong_tac2 bit_is_clear[PING,1]// 1
//-=====khai bao bien cho ham do duong moi====
#define cb0 bit_is_set[PINF,0]
#define cb1 bit_is_set[PINF,1]
#define cb2 bit_is_set[PINF,2]
#define cb3 bit_is_set[PINF,3]
#define cb4 bit_is_set[PINF,4]
#define cb5 bit_is_set[PINF,5]
#define cb6 bit_is_set[PINF,6]
#define cb7 bit_is_set[PINF,7]
#define cb PINF
#define un unsigned
#define vo volatile
// ====KHAI BAO CHO DAO CHIYEU DONG CO======
#define banhtrai_dao cbi[PORTB,3];
#define banhtrai_thuan sbi[PORTB,3];
#define banhphai_dao cbi[PORTB,4];
#define banhphai_thuan sbi[PORTB,4];
// Khai bao cho do duong
unsigned char so_line,so_lines;
un char val_pwm,val_pwm_t,nho_line,pwm_speed;
un char tam_pwm,bit_vung,bit_nhanh_cham,pwm_80,pwm_90,pwm_70,pwm_60,pwm_50,pwm_30,pwm_20;
un char L[10],R[10];
un char Ls[10],Rs[10];
un char Lmpu[9],Rmpu[9];
char str[8];
//====================khai bao cho follow line PID========================
//==========================macro set,clear bit============================//
#ifndef cbi
#define cbi[port, bit] [port] &= ~[1 pin_bit] & 1] //ok dung
#endif
#ifndef in_Pin_Init
#define in_Pin_Init[DDRx, DDBx] [DDRx] &= ~[1 PWM2 onboard
OCR3CL=0; //=> PWM3 onboard
//=============================khai bao cho UART====================================
//=========================== USART0 initialization
// USART0 disabled
UCSR0B=0x00;
//============================= USART1 initialization
// Communication Parameters: 8 Data, 2 Stop, No Parity
// USART1 Receiver: On
// USART1 Transmitter: On
// USART1 Mode: Asynchronous
// USART1 Baud Rate: 9600
UCSR1A=0x00;
UCSR1B=0x98;
UCSR1C=0x0E;
UBRR1H=0x00;
UBRR1L=0x67;
// External Interrupt[s] initialization
// INT0: Off
// INT1: Off
// INT2: Off
// INT3: Off
// INT4: Off
// INT5: Off
// INT6: On
// INT6 Mode: Any change
// INT7: On
// INT7 Mode: Any change
EICRA=0x00;
EICRB=0x50;
EIMSK=0xC0;
EIFR=0xC0;
sei[]; // cho phep ngat toan cuc
//========================DHVT1B==================================//
while[1]
{
chay_do_lai[60,2];
Banh_trai=0;
Banh_phai=0;
while[1];
}
}
// ******************chuong trinh con********************************//
//=======cac ham dieu khien dong co moi==================
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
void tien_trai [unsigned char e2]
{
banhtrai_thuan;
Banh_trai=e2;
}
//xxxxxxxxxxxxxxxxxxxxxxxxxxxx
void tien_phai [unsigned char e2]
{
banhphai_thuan;
Banh_phai=e2;
}
//================CAC HAM DO LINE============================================//
un char dem_cb[]
{ un char i=0;
if[!cb0] i++; if[!cb1]i++; if[!cb2] i++; if[!cb3] i++;
if[!cb4] i++;if[!cb5] i++;if[!cb6] i++;if[!cb7] i++;
return i;
}
//==============================================
void dem_vach_ko_encoder[]// 1 lai 520 XUNG
{
if[dem_cb[]>4]
{so_line++; }
}
//==============================================
void do_lai[void]// cb7 ben phai , cb0 ben trai
{
// thang nhat ==maxx
if[cb==0 || [!cb3&&!cb4]]
{tien_phai[R[0]];tien_trai[L[0]]; nho_line=4;} //***XX***di max
//===================== ket hop cam bien sau====================
else if [!cb3 && cb2 && cb4]
{tien_phai[R[0]];tien_trai[L[1]];nho_line=4;}//***X****
else if [!cb4 && cb3 && cb5]
{tien_phai[R[1]];tien_trai[L[0]];nho_line=4;}//****X***
else if [!cb3 && !cb2]
{tien_phai[R[0]];tien_trai[L[2]];nho_line=0;}//**XX****
else if [!cb4 && !cb5]
{tien_phai[R[2]];tien_trai[L[0]];nho_line=0;}//****XX**
else if[!cb2 && cb1 && cb3]
{tien_phai[R[0]];tien_trai[L[3]];nho_line=0;}//**X*****
else if [!cb5 && cb4 &&cb6]
{tien_phai[R[3]];tien_trai[L[0]];nho_line=0;}//*****X**
else if [!cb2 && !cb1 ]
{tien_phai[R[0]];tien_trai[L[4]];nho_line=0;}//*XX*****
else if [!cb5 && !cb6 ]
{tien_phai[R[4]];tien_trai[L[0]];nho_line=0;}//*****XX*
else if[!cb1 && cb0 && cb2]
{tien_phai[R[0]];tien_trai[L[5]];nho_line=0;}//*X******
else if [!cb6 && cb7 && cb5]
{tien_phai[R[5]];tien_trai[L[0]];nho_line=0;}//******X*
else if [!cb1 && !cb0 ]
{tien_phai[R[0]];tien_trai[L[6]];nho_line=1;}//******XX
else if [!cb6 && !cb7 ]
{tien_phai[R[6]];tien_trai[L[0]];nho_line=7;}//XX******
else if[!cb0 && cb1]
{tien_phai[R[0]];tien_trai[L[6]];nho_line=1;}//*******X
else if [!cb7 && cb6]
{tien_phai[R[6]];tien_trai[L[0]];nho_line=7;}//X*******
else
{
if[nho_line==4] {tien_phai[20];tien_trai[20];}
else if[nho_line==7] {tien_phai[0];tien_trai[10];}
else if[nho_line==1] {tien_phai[10];tien_trai[0];}
}
}
void speed60[]//120
{ // 34321 0
L[0]=250;L[1]=150;L[2]=100;L[3]=50;L[4]=30;L[5]=20;L[6]=10;
R[0]=250;R[1]=150;R[2]=100;R[3]=50;R[4]=30;R[5]=20;R[6]=10;
}
void speed70[]//120
{ // 34321 0
L[0]=56;L[1]=53;L[2]=53;L[3]=53;L[4]=53;L[5]=53;L[6]=53;
R[0]=60;R[1]=54;R[2]=54;R[3]=54;R[4]=54;R[5]=54;R[6]=54;
}
void speed80[]//120
{ // 34321 0
L[0]=56;L[1]=53;L[2]=53;L[3]=53;L[4]=53;L[5]=53;L[6]=53;
R[0]=60;R[1]=54;R[2]=54;R[3]=54;R[4]=54;R[5]=54;R[6]=54;
}
void set_speed[un char a]
{
if[a==60] speed60[];
else if[a==70] speed70[];
else if[a==80] speed80[];
}
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
void chay_do_lai[unsigned char a, unsigned char line_can_di ]
{
set_speed[a];
so_line=0;
while[1]
{
do_lai[];
dem_vach_ko_encoder[];
if[so_line==line_can_di] break;
}
Banh_trai=0;
Banh_phai=0 ;
}
//=============================================
void stop_pid [void]
{
Banh_trai=2 ;
Banh_phai=2 ;
}
Mô phỏng code trên Proteus
Hướng dẫn sử dụng mô phỏng:
Thực hiện chọn đóng các công tắc lần lượt từ 2 bóng giữa sang bên trái hoặc bên phải [ mô phỏng quá trình từ chạy thẳng cho đến khi bị lệch khỏi đường line, chú ý cùng 1 lúc chỉ được tối đa 2 bóng led liên tiếp nhau sáng]
Link dowload mã nguồn trong Project
code do duong
File mô phỏng Proteus
Số lượt xem: 635
Tags: dò đường roboconfollow line robocnlập trình dò đườnglập trình dò line