Giáo trình Robocon tóm tắt Trương Trọng Toại Buổi TÌM HIỂU VỀ CÁC CẢM BIẾN TRÊN ROBOT Như buổi đề cập ,cảm biến robot bố trí thành tầng (Tầng ,tầng ,và tầng ).Buổi hôm ta sâu vào loại cảm biến 1.Cảm biến tầng : Trong Robocon tầng thường trang bị Camera Ta tìm hiểu kĩ CMU Cam Ưu điểm Camera tích hợp chip xử lý ảnh sẵn nên việc ứng dụng dễ dàng Chỉ cần dùng loại vi điều khiển thơng thường giao tiếp tốt với CMU Cam www.nhom3c.com Giáo trình Robocon tóm tắt Trương Trọng Toại (Hướng dẫn sử dụng phần mềm hỗ trợ rõ CMUCam Manual ) Chương trình tham thảo: Dùng PIC16F877A giao tiếp với CMU Cam // Spot Welding Mobile Robot V2 (Only Spot, No Welding Line) // PIC877: (1)for Welding Process; (2) for Tracking Line using Camera sensor // PIC Comm.: Parallel (PSP) // Camera Comm.: RS232 // Optimal Code: OK //****************************** /* Parameters: (1) Rmin;Rmax;Gmin;Gmax;Bmin;Bmax (2) Sensitive */ //****************************** #include #include #include #include #fuses hs, nowdt, noprotect, put, nolvp, brownout #use delay(clock=20000000) #use rs232(baud=115200,parity=N,xmit=PIN_C6,rcv=PIN_C7) #byte pie2 = 0x8c #byte rcsta = 0x18 www.nhom3c.com Giáo trình Robocon tóm tắt Trương Trọng Toại int16 Count1,Count2; int16 WeldingSpeed=250; char Packet=' '; int16 mx=43; int16 my=0; int8 x1=0; int8 y1=0; int8 x2=0; int8 y2=0; int16 pixels=0; int16 confidence=0; int8 Rmin=160; int8 Rmax=255; int8 Gmin=170; int8 Gmax=255; int8 Bmin=100; int8 Bmax=255; int8 Sensitive=2; int max=7; int pos=0; char c; char s2[30]; int1 index=0; int16 duty=300; int16 duty1=300; //**************************************** void get_string2(char * s,int max) { int len; char c; max ; len=0; { c=getc(); if ((c>=' ')&&(c= 250){m=1;} if(tocdolx>=m){klen=0;} else {klen=1;} if(tocdok>=m){kqua=0;} else{kqua=1;} ///////////////////////////////////////////////////////////////// if ((P0 & 0x3C) == 0x3C) {finish=1;} if ((P0 & 0x07) == 0x07) {finish1=1;} if ((P0 & 0xF0) == 0xF0) {finish1=1;} if ((P0 & 0xE0) == 0xE0) {finish1=1;} /////////////////////////////////////////////////////////////////////////////////////// if(en_doduong) { switch(P0) {// unsigned char tocdol,tocdor; //********************************************************************** ********** case 0xF3: // co cam bien ben trai dk ve ben trai { nho_trai=1; nho_phai=0; finish = 1; tocdol = tocdo - tocdo/6; tocdor = tocdo + tocdo/4; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; // motor phai chay toi CCAP2H = tocdol; // motor trai chay toi CCAP1H = 0x00; CCAP3H = 0x00; } break; //********************************************************************** *************** www.nhom3c.com Giáo trình Robocon tóm tắt Trương Trọng Toại case 0xCF: // co cam bien ben phai dk ve ben phai { finish = 1; nho_trai = 0; nho_phai = 1; tocdol = tocdo + tocdo/4; tocdor = tocdo - tocdo/6; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //********************************************************************** ********** case 0xFF: //ko co cam bien { finish=1; tocdol = tocdo/3; tocdor = tocdo/3; // chaytoi(tocdol,tocdol); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //********************************************************************** ********** case 0x00: //ko co cam bien { if (nho_trai) { // chaylui(15,0) ve ben phai P1 = 0xFF; dir1 = 0; dir3 = 0; CCAP4H = 0x00; www.nhom3c.com Giáo trình Robocon tóm tắt Trương Trọng Toại CCAP2H = 0x00; CCAP1H = 0; CCAP3H = 25; //dong co phai lui } else if (nho_phai) { //chaylui(0,15); P1 = 0xFF; dir1 = 0; dir3 = 0; CCAP4H = 0x00; CCAP2H = 0x00; CCAP1H = 25; // dong co trai lui CCAP3H = 0; } else { tocdol = 0; tocdor = 0; // chaytoi(tocdol,tocdol); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } } break; //********************************************************************** *************** //********************************************************************** *************** case 0x01: // co cam bien senln2 dieu khien lech toc toan bo ve trai { nho_trai=1;nho_phai=0; tocdol = 40; tocdor = 200; // quaytrai1(tocdol,tocdor); P1 = 0xFF; www.nhom3c.com Giáo trình Robocon tóm tắt Trương Trọng Toại dir1 = 0; dir2 = 0; CCAP4H = tocdol; CCAP2H = 0x00; CCAP1H = tocdor; CCAP3H = 0x00; } break; case 0x80: // co cam bien senrn2 dieu khien lech toc toan bo ve phai { nho_trai=0;nho_phai=1; tocdol = 200; tocdor = 40; // quayphai1(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir3 = 0; CCAP4H = 0x00; CCAP2H = tocdor; CCAP1H = 0x00; CCAP3H = tocdol; } break; //********************************************************************** ******* //********************************************************************** *********** case 0xC0: // co cam bien senln1 + senln2 dieu khien lech toc vua ve trai { nho_trai=1;nho_phai=0; tocdol = 150; tocdor = 60; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; case 0x03: // co cam bien senrn1 + senrn2 dieu khien lech toc vua ve phai www.nhom3c.com Giáo trình Robocon tóm tắt Trương Trọng Toại { nho_trai=0;nho_phai=1; tocdol = 60; tocdor = 150; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //********************************************************************** ******* case 0x02: // co cam bien senln1 dieu khien lech toc nhieu ve trai { nho_trai=1;nho_phai=0; tocdol = tocdo - tocdo/3 ; tocdor = tocdo + tocdo/5; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; case 0x40: // co cam bien senrn1 dieu khien lech toc nhieu ve phai { nho_trai=0;nho_phai=1; tocdol = tocdo + tocdo/5 ; tocdor = tocdo - tocdo/3; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; www.nhom3c.com Giáo trình Robocon tóm tắt Trương Trọng Toại } break; //********************************************************************** ********* case 0x06: // co cam bien senln + senln1 dieu khien lech toc vua ve trai { nho_trai=1;nho_phai=0; tocdol = (2*tocdo)/3;//35;//30 tocdor = tocdo + tocdo/4;//30; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; case 0x60: // co cam bien senrn + senrn1 dieu khien lech toc vua ve phai { nho_trai=0;nho_phai=1; tocdol = tocdo + tocdo/4;//30 tocdor = 2*tocdo/3;//30; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //********************************************************************** ********* case 0x04: // co cam bien senln dieu khien lech toc vua ve trai { nho_trai=1;nho_phai=0; tocdol = tocdo-(1*tocdo)/3; tocdor = tocdo+tocdo/4; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; www.nhom3c.com Giáo trình Robocon tóm tắt Trương Trọng Toại CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; case 0x20: // co cam bien senrn dieu khien lech toc vua ve phai { nho_trai=0;nho_phai=1; tocdol = tocdo+(tocdo/4); tocdor = tocdo-(1*tocdo)/3; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //********************************************************************** **** case 0x0C: // co cam bien senlt va senln dieu khien lech toc vua ve trai { nho_trai=1;nho_phai=0; tocdol = tocdo - tocdo/4; tocdor = tocdo + tocdo/4; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; case 0x30: // co cam bien senrt + senrn dieu khien lech toc vua ve phai { nho_trai=0;nho_phai=1; tocdor = tocdo - tocdo/4; tocdol = tocdo + tocdo/4; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; www.nhom3c.com Giáo trình Robocon tóm tắt Trương Trọng Toại dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //********************************************************************** *************** case 0x08: //3 co cam bien senlt dieu khien lech toc it ve trai { nho_trai=1;nho_phai=0; tocdol = tocdo - tocdo/6; tocdor = tocdo + tocdo/6; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; case 0x10: // co cam bien senrt dieu khien lech toc it ve phai { nho_trai=0;nho_phai=1; tocdol = tocdo + tocdo/6; tocdor = tocdo - tocdo/6; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //********************************************************************** *********** www.nhom3c.com Giáo trình Robocon tóm tắt Trương Trọng Toại //********************************************************************** *********** case 0x18: //2 cam bien giua senlt + senrt { nho_trai=0;nho_phai=0; tocdol = tocdo; tocdor = tocdo; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } break; //********************************************************************** ****** default: // truong hop khong co cam bien nao { tocdol = tocdo/3;//-(2*tocdo)/3; tocdor = tocdo/3;//-(2*tocdo)/3; // chaytoi(tocdol,tocdor); P1 = 0xFF; dir0 = 0; dir2 = 0; CCAP4H = tocdor; CCAP2H = tocdol; CCAP1H = 0x00; CCAP3H = 0x00; } } if ((P0 & 0x3C) == 0x3C) {finish=1;} if ((P0 & 0x07) == 0x07) {finish1=1;} if ((P0 & 0xF0) == 0xF0) {finish1=1;} if ((P0 & 0xE0) == 0xE0) {finish1=1;} } ///////////////////////////////////////////////////////////////////////////////////////// www.nhom3c.com Giáo trình Robocon tóm tắt Trương Trọng Toại } www.nhom3c.com ... PSEN 28 27 26 25 24 23 22 21 P2.7 P2.6 P2.5 P2.4 P2.3 P2 .2 P2.1 P2.0 RST 4k7 EA/VPP P3.0/RXD P3.1/TXD ALE/PROG PSEN P3 .2/ INT0 P3.3/INT1 P3.4/T0 P2.7/A15 P3.5/T1 P2.6/A14 P3.6/WR P2.5/A13 P2.4/A 12. .. LM339 2OUT2OUT2 2OUT1OUT1 5V +V 2- IN1 -IN1 2+ IN1 +IN1 2- IN2 -IN2 2+ IN2 +IN2 OUT32OUT3 OUT42OUT4 GND +IN4 2+ IN4 2- IN4 -IN4 +IN3 2+ IN3 2- IN3 -IN3 14 13 12 11+ 109+ 8- D33 D33 D33 LED P0.5 R9 R 2OUT3... P1 .2 P1.3 P1.4 P1.5 P1.6 P1.7 P1.0 P1.1 P1 .2 P1.3 P1.4 P1.5 P1.6 P1.7 J4 P2.0 P2.1 P2 .2 P2.3 P2.4 P2.5 P2.6 P2.7 Port2 www.nhom3c.com Port3 P3.0 P3.1 P3 .2 P3.3 P3.4 P3.5 P3.6 P3.7 Giáo trình Robocon