Sơ đồ đấu nối phần cứng
Các khai báo cần thiết
|
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.
|
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.
|
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
|
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 <stdio.h> #include <string.h> #include <stdlib.h> #include <avr/io.h> #include <avr/interrupt.h> #include <util/delay.h> #include <avr/eeprom.h> #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 << (bit)) // macro cho clear bit #endif #ifndef sbi #define sbi(port, bit) (port) |= (1 << (bit)) // macro cho set bit #endif //===========================macro read bit,============================// #ifndef read_bit #define read_bit(port_pin, pin_bit) ((port_pin >> pin_bit) & 1) //ok dung #endif #ifndef in_Pin_Init #define in_Pin_Init(DDRx, DDBx) (DDRx) &= ~(1 << (DDBx)) //set pin la "in" or "out" #endif #ifndef out_Pin_Init #define out_Pin_Init(DDRx, DDBx) (DDRx) |= (1 << (DDBx)) #endif #ifndef use_r_up #define use_r_up(port, bit) (port) |= (1 << (bit)) #endif // ========================macro cho phep/ khong cho phep ngat===========================// // su dung cho ngat ngoai INT0-INT7 // int0-cong tac len // int1-cong tac xuong // int2-UART1=>ko dung // int3-UART1=>ko dung // int4-dem encoder len xuong 4 trang thai // int5-dem encoder len xuong 4 trang thai // int6-dem encoder banh xe 4 trang thai // int7-dem encoder banh xe 4 trang thai #define en_int(bit) EIMSK |=(1<<(bit)) // dung cho ngat cong tac len or SCL #define dis_int(bit) EIMSK &= ~(1<<(bit)) // dung cho ngat cong tac len or SCL //*********************************************Ham main*********************************************************// int main(void) { DDRA=0xFF; //====== DDRB=0xFE; //======= DDRC=0; // sua de test //======= DDRE=0; //3f //======= DDRD=0XF0; //======= DDRG=0x00; //======================================================// PORTD=0XFF; PORTA=0xFF; PORTB=0xFF; PORTC=0xFF; PORTE=0xFF; // Pull all for PWM, INT, Program without error PORTG=0xFF; //=============================== khai bao timer 1 cho PWM4,5,6================== TCCR1A=0xFD;//(WGM13=0, WGM12=0, WGM11=0, WGM10=1) giá tri TOP là 1 hang so, TOP = 255 TCCR1B=0x04; // COMA1=COMA0=COMB1=COMB0// GIA TRI XUNG RA HIGH(COMPARE) LOW(TOP) OCR1AL=0; //pwm1A value => PWM4 onboard OCR1BL=0; //pwm1B value => PWM5 onboard OCR1CL=0; //=> PWM6 onboard /* //=============================== khai bao cho timer 2 dung de dem thoi gian====== // Timer/Counter 2 initialization // Clock source: System Clock // Clock value: 62.500 kHz // Mode: Normal top=FFh // OC2 output: Disconnected TCCR2=0x04; //04 bo chia 256 TCNT2=0x00;// gia tri tran OCR2=0x00; */ //============================ khai bao timer 3 cho PWM1,2,3====================== TCCR3A=0; // 0xfd//(WGM13=0, WGM12=0, WGM11=0, WGM10=1) giá tri TOP là 1 hang so, TOP = 255 TCCR3B=0; // 0x04// COMA1=COMA0=COMB1=COMB0// GIA TRI XUNG RA HIGH(COMPARE) LOW(TOP) OCR3AL=0; //pwm1A value => PWM1 onboard OCR3BL=0; //pwm1B value => 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 { // 34 3 2 1 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 { // 34 3 2 1 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 { // 34 3 2 1 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)
![[Robocon cơ bản]- Thiết kế mạch điều khiển cơ bản với AVR Atmega64/128](https://luuvachiase.net/wp-content/uploads/2019/11/thiet-ke-mach-robot-avr6-120x86.png?v=1573485942)
![[Robocon cơ bản]- Thiết kế mạch điều khiển cơ bản với AVR Atmega64/128](https://luuvachiase.net/wp-content/uploads/2019/11/set-fulse-bit-avr-atmega-voi-burn-e-120x86.png?v=1573485937)
![[Robocon cơ bản]- Thiết kế mạch điều khiển xilanh và role đơn giản với ULN2803](https://luuvachiase.net/wp-content/uploads/2019/11/PCB-dieu-khien-xilanh-e1573483900548-120x86.png?v=1573485936)
Anh ơi anh có thể giải thích lại chỗ mảnh vận tốc đk ko ạ
Trước khi sử dụng hàm dò đường, em cần nạp giá trị vận tốc cho hàm này, mà cụ thể là mảng vận tốc đã khai báo speed60, speed70… Các mảng vận tốc này có giá trị được gán theo qui luật là các giá trị đầu thường là sẽ cao, sau đó giảm dần, hoặc giảm vài giá trị rồi giữ nguyên. Tùy thuộc vào đặc tính của robot mà em sẽ điều chỉnh tham số trong mảng này để robot có thể di chuyển linh hoạt, không bị giật, đánh võng trong quá trình chạy, nói chung cái này sẽ cần kinh nghiệm, còn kĩ thuật cao hơn thì cần đến thuật toán PID.
Vâng ạ em câm ơn anh nhiều ạ