Robot tự động đi theo người

L i gi i thi uờ ớ ệ Trong xã h i ngày nay, kĩ thu t đi n t đã và đang ng d ng m nh m trong nhi u lĩnh v c,ộ ậ ệ ử ứ ụ ạ ẽ ề ự và ngày càng tr nên ph bi n h n là lĩnh v c “Đi u Khi n T Đ ng”. Trong nh ng côngở ổ ế ơ ự ề ể ự ộ ữ trình l n nh nhà máy, xí nghi p chúng ta th y rõ h n vai trò quan tr ng c a đi u khi n tớ ư ệ ấ ơ ọ ủ ề ể ự đ ng ho c nh ng ng d ng t đ ng trong lĩnh v c dân d ng, có nh ng máy thông minh ph cộ ặ ữ ứ ụ ự ộ ự ụ ữ ụ v cho nh ng nhu c u sinh ho t cho con ng i . t t c nh ng máy móc đó đi u đ c đi uụ ữ ầ ạ ườ ấ ả ữ ề ượ ề khi n b i b x lí trung tâm, nh ng h th ng l n thì b x lí trung tâm là PLC, máy tínhể ở ộ ử ở ữ ệ ố ớ ộ ử công nghi p.., nh ng h th ng x lí nh h n thì ng i ta dùng vi đi u khi n.ệ ở ữ ệ ố ử ỏ ơ ườ ề ể V i m t vi đi u khi n , khi k t h p v i m t c c u ch p hành h p lí, ng i l p trình có thớ ộ ề ể ế ợ ớ ộ ơ ấ ấ ợ ườ ậ ể đi u khi n m t cách linh ho t c c u đó theo yêu c u đ t ra , làm cho mô hình ho t đ ng linhề ể ộ ạ ơ ấ ầ ặ ạ ộ ho t h n, thông minh h n. Chính vì y u t này nên có nhi u nhà s n xu t cho vi đi u khi nạ ơ ơ ế ố ề ả ấ ề ể ,t o nên nh ng dòng vi đi u khi n có tính năng c nh tranh v i nhau và các vi đi u khi n liênạ ữ ề ể ạ ớ ề ể t c đ c c i ti n. Và PIC c a hãng MICROCHIPụ ượ ả ế ủ là m t trong nh ng dòng vi đi u khi n cóộ ữ ề ể tính năng m nh m mà đ i v i sinh viên Vi t Nam đã b t đ u quen thu c.ạ ẽ ố ớ ệ ắ ầ ộ Bên c nh vi đi u khi n , c m bi n là y u t quan tr ng không kém trong m t c c u hayạ ề ể ả ế ế ố ọ ộ ơ ấ m t h th ng, nó nh là “tay”, “m t” c a m t c c u. Emộ ệ ố ư ắ ủ ộ ơ ấ ng d ngứ ụ nh ng y u t này vàoữ ế ố mô hình “Xe Robot Bám Theo Ng i”. Tuy nhiên do ki n th c, kinh nghi m và th i gian cóườ ế ứ ệ ờ h n nên đ tài em còn nhi u khuy t đi m, kính mong quý th y, cô cùng b n bè đóng góp ýạ ề ề ế ể ầ ạ ki n đ đ tài đ c hoàn thi n h n.ế ể ề ượ ệ ơ CH NG IƯƠ Lí Thuy tế 1.1/Vi đi u khi n PIC 16F877Aề ể A/ S đ chân :ơ ồ B/S đ kh i :ơ ồ ố Tóm l c ph n c ng PIC 16F877A :ượ ầ ứ -T n s th ch anh 20 MHz.ầ ố ạ -B nh ROM n i :8 KB(Flash ROM).ộ ớ ộ -386 BYTE RAM n i.ộ -256 BYTE EFROM trong vùng ROM n i dùng ch a d li u.ộ ứ ữ ệ -V giao ti p, có 5 portề ế A, port B, port C, port D, port E vào ra v i tín hi u đi u khi n đ cớ ệ ề ể ộ l p.ậ -2 b đ nh th i :Timer 0 và Timer 2 đ phân gi i 8 bit.ộ ị ờ ộ ả - 1 b đ nh th i Timer 1 đ phân gi i 16 bit.ộ ị ờ ộ ả -2 CCP(Capture/Compare/PWM). -1 b ADC 10 bit v i 8 ngõ vào.ộ ớ -1 b Watch Dog Timer .ộ -15 ngu n ng tồ ắ -N p ch ng trình thông qua c ng n i ti p(ICSP-In Circuit Serial Programming).ạ ươ ổ ố ế -Cũng như các vi đi u khi n khác ta c p dao đ ng cho PIC, chân ngu n, chân Reset. PICề ể ấ ộ ồ Ở 16F877A : +Chân 1: reset tích c c m c th p.ự ứ ấ +Chân 13,14 c p dao đ ng ,có th dùng th ch anh 20MHz,4 MHzấ ộ ể ạ + Chân 12,31 c p ngu n 0 V.(Vss)ấ ồ + Chân 11,32 c p ngu n 5 V(Vdd)ấ ồ +Các chân còn l i x p thành 5 port xu t nh p nh s đ trên.ạ ế ấ ậ ư ơ ồ -Các ch c năng ph n c ng c a PIC đ c h tr trong ph n m m l p trình: cách thi t đ t portứ ầ ứ ủ ượ ổ ợ ầ ề ậ ế ặ xu t nh p, th vi n h tr ph n c ng ,khai báo t n s th ch anh , đ t đ a ch tín hi u vàoấ ậ ư ệ ổ ợ ầ ứ ầ ố ạ ặ ị ỉ ệ ra...Nh v y ng i l p trình s d dàng h n trong thao tác l p trình, s d ng h p lí các ô như ậ ườ ậ ẽ ễ ơ ậ ử ụ ợ ớ ,đ a ch c n thi t. Tùy theo m i dòng PIC mà nhà s n xu t tích h p nh ng module chuyênị ỉ ầ ế ỗ ả ấ ợ ữ d ng, ví d module PWM, ADC,.., khi mu n s d ng module nào ta ch c n khai báo và thi tụ ụ ố ử ụ ỉ ầ ế l p ph n m m và ng d ng. chính vì y u t này nên ngày càng nhi u ng i quan tâm đ nậ ầ ề ứ ụ ế ố ề ườ ế PIC b i tính hi u qu và d s d ng c a nó.ở ệ ả ễ ử ụ ủ 1.2/ Ch ng trình l p trình cho PIC : PIC C COMPILERươ ậ Ch ng trình có c u trúc l nh gi ng C++ , file g c là C file, sau khi biên d ch(compile) t o raươ ấ ệ ố ố ị ạ HEX file và ASM file, h tr s a l i. có th vi t ch ng trình cho nhi u dòng PIC nh dòngỗ ợ ữ ỗ ể ế ươ ề ư PIC 12bit(PCB), PIC 14bit(PCM), PIC 16bit(PCH),PIC 24bit(PCD), PIC 16F877A là dòng 14bit, khi vi t ch ng trình ta ph i ch n h tr cho t ng dòng PIC, n u không biên d ch fileế ươ ả ọ ỗ ợ ừ ế ị không có ý nghĩa. Trong m i ch ng trình ta ph i khai báo th vi n h tr cho chip đang l pỗ ươ ả ư ệ ỗ ợ ậ trình, t n s th ch anh s d ng,các th vi n ph tr khác, h tr “HELP” là m t công c đ cầ ố ạ ử ụ ư ệ ụ ợ ỗ ợ ộ ụ ắ l c cho ng i l p trình, v i m i ch c năng c a PIC ta có th tìm cách khai báo , thi t l p, sự ừơ ậ ớ ọ ứ ủ ể ế ậ ử d ng trong “HELP”. ụ 1.3/ Ch ng Trình N p : WIN PIC800ươ ạ Ch ng trình n p đa năng cho nhi u dòng PIC , ta ph i ch n dòng chip , ví d nh 16F877A,ươ ạ ề ả ọ ụ ư ch n nh hình v ,ọ ư ẽ sau đó ch n đúng tên chip mình đang n p, ch n setting/hardware/JDMọ ạ ọ programmer: ế Do PIC đ c h tr t i đa t ph n m m nên các thao tác vi t ch ng trình và n p ch ngượ ỗ ợ ố ừ ầ ề ế ươ ạ ươ trình vào chip ph i chính xác.ả 1.4/ C m Bi n Siêu Âm SRF05 :ả ế A Hình Dáng Hình 1.4.1 SRF05 là c m bi n siêu âm g m m ch phát và m ch thu trên cùng m t board, có th nh nả ế ồ ạ ạ ộ ể ậ d ng đ c nhi uạ ượ ề v t c n và nhi u kho ng cách, có th đo đ c kho ng cách t 4cm -> 4m .ậ ả ề ả ể ượ ả ừ V i 2 ch đ ho t đ ng (mode 1,mode2). Trong đ tài em ch n ch đ ho t đ ng mode 1.ớ ế ộ ạ ộ ề ọ ế ộ ạ ộ B Nguyên Lí Ho t Đ ng:ạ ộ ch đ h at đ ng mode 1 , chân mode trên board đ l ngở ế ộ ọ ộ ể ữ không k t n i nh s đ chânế ố ư ơ ồ trên. Gi n đ xung nh sau:ả ồ ư ế Mu n SRF05 ho t đ ng, ta c p ngu n vào cho nó, khi mu n đo kho ng cách ,ta c p vào chânố ạ ộ ấ ồ ố ả ấ trigger xung có TON >=10uS, SRF05 đ c l p trình s n ,nó phát ra 8 burst siêu âm, khi sóngượ ậ ẵ siêu âm g p v t c n ,ph n h i v m ch thu siêu âm trên Board, lúc này tùy theo kho ng cáchặ ậ ả ả ồ ề ạ ả gi a v t c n và board ,SRF05 tr v m t xung có TON thay đ i 100uS --> 25mS, n u khôngữ ậ ả ả ề ộ ổ ế có v t c n thì TON =30mS.ậ ả Mu n đo kho ng cách th c t ta đo giá tr xung ph n h i, đ i ra uS, sau đó chia cho 58 đ l yố ả ự ế ị ả ồ ổ ể ấ giá tr đo là cm, chia cho 148 l y giá tr là inchị ấ ị T n s kích chân Trigger t i đa là 20Hz, đ ng nghĩa ta có th c p nh t giá tr đo nhanh nh tầ ố ở ố ồ ể ậ ậ ị ấ là 20 l n/giây.ầ 1.5 C m Bi n Chuy n Đ ng PIR 325ả ế ể ộ A Gi i thi u v h ng ngo iớ ệ ề ồ ạ Tia h ng ngo i t n t i trong ph tr ng đi n t ,có b c sóng dài h n b c sóng c a ánhồ ạ ồ ạ ổ ườ ệ ừ ướ ơ ướ ủ sáng có th nhìn th y đ c. Tia h ng ngo i không th nhìn th y nh ng có th phát hi n đ cể ấ ượ ồ ạ ể ấ ư ể ệ ượ nh vào nh ng trang thi t b chuyên d ng. nh ng v t sinh ra nhi t năng đ u sinh ra tia h ngờ ữ ế ị ụ ữ ậ ệ ề ồ ngo i, bao g m c đ ng v t và con ng i, b c sóng c a tia h ng ngo i do con ng i sinh raạ ồ ả ộ ậ ườ ướ ủ ồ ạ ườ vào kho ng 9.4 m.ả B C u trúc và ho t đ ng PIR325ấ ạ ộ 1 s đ kh iơ ồ ố Hình 1.5.1 Thông s PIR325ố ế 2 Nguyên lí ho t đ ng và cách b tríạ ộ ố Hình 1.5.2 Nguyên t c chung c a PIR là thu tia h ng ngo i do con ng i hay đ i t ng t o ra h ngắ ủ ồ ạ ườ ố ượ ạ ồ ngo i di chuy n, trong đ u thu PIR có 2 phi n c m ng h ng ngo i, khi có tia h ng ngo iạ ể ầ ế ả ứ ồ ạ ồ ạ đ c h p th , 2 phi n này t o ra 1 áp r t nh g n nh tín hi u nhi u và có đ l ch nhau, 2ượ ấ ụ ế ạ ấ ỏ ầ ư ệ ễ ộ ệ tín hi u này đ c đ a vào chân FET nh hình 1.5.1, t ng này tín hi u đ c khu ch đ i 100ệ ượ ư ư ở ầ ệ ượ ế ạ l n, t ng 2 khu ch đ i 100 l n, sau khi khu ch đ i tín hi u đi vào m ch so sánh , t i đây tínầ ở ầ ế ạ ầ ế ạ ệ ạ ạ hi u đ c l c nhi u và cho tín hi u ra 3.9 VDCệ ượ ọ ễ ệ ở Ch ng IIươ THI CÔNG: 2.1 Nhi m v và c s lý lu n phân tích đ tài:ệ ụ ơ ở ậ ề A/Yêu c u đ tài:ầ ề Xe g m 2 đ ng c DC lái h ng xe,vi x lí lái đ ng b 2 đ ng c đi u khi n xe ch y theoồ ộ ơ ướ ử ồ ộ ộ ơ ề ể ạ ng i. Nguyên lí phát hi n s di chuy n ng i dùng sóng h ng ngo i do b c m bi n PIRườ ệ ự ể ườ ồ ạ ộ ả ế đ m nh n,m ch lái công su t dùng mostfet và r le 2 ti p đi m đi u khi n đ ng c . C mả ậ ạ ấ ơ ế ể ề ể ộ ơ ả bi n nh n d ng s thay đ i h ng ngo i khi ta di chuy n báo v vi x lí , vi x líế ậ ạ ự ổ ồ ạ ể ề ử ử đi u khi nề ể xe ch y theo ng i có s di chuy n đó.ạ ườ ự ể B / Phân tích đ tài:ề Xe có th t cân b ng , ch y th ng, qu o trái, thi t k xe 3 bánh xe, bánh tr c cân b ng vàể ự ằ ạ ẳ ẹ ế ế ướ ằ có th quay đa h ng , 2 bánh sau đ c truy n đ ng c p moment chuy n đ ngể ướ ượ ề ộ ấ ể ộ cho xe, ch nọ lo i đ ng c có công su t phù h p v i khung xe, truy n đ ng đ n gi n nên ch n lo i đ ngạ ộ ơ ấ ợ ớ ề ộ ơ ả ọ ạ ộ c có h p s . Sau khi có giàn khung ta c n c c u đi u khi n nên ph i có “M ch X Líơ ộ ố ầ ơ ấ ề ể ả ạ ử Trung Tâm”, nhi m v m ch x lí là đi u khi n xe ch y tùy theo giá tr đi u khi n nh nệ ụ ạ ử ề ể ạ ị ề ể ậ đ c. Yêu c u đ tài là “ Xe Bám Theo Ng i”, tr c khi bám theo ng i,xe c n ph iượ ầ ề ườ ướ ườ ầ ả “Th y” ng i , nguyên lí dùng c m bi n PIR nh n d ng h ng ngo i c a con ng i khi ng iấ ườ ả ế ậ ạ ồ ạ ủ ườ ườ đó di chuy n. Khi xe ch y v h ng có ng i nh ng không đ ng vào ng i đó,v y c n cóể ạ ề ướ ườ ư ụ ườ ậ ầ m ch dò kho ng cáchạ ả gi a ng i và xe, đây em dùng c m bi n Siêu Âm đ đo kho ngữ ườ ở ả ế ể ả cách gi a ng i và xe. Trên xe b trí m ch đi u khi n vô tuy n đ kh i đ ng và d ng xe.ữ ườ ố ạ ề ể ế ể ở ộ ừ Gi a m ch đi u khi n và đ ng c có m ch Công Su t Cách Li đ m b o tín hi u đi u khi nữ ạ ề ể ộ ơ ạ ấ ả ả ệ ề ể n đ nh.ổ ị T cách phân tích trên em đ a ra s đ kh i t ng th cho xe nh hình 2.1ừ ư ơ ồ ố ổ ể ư ế *S Đ KH I :Ơ Ồ Ố Hình 2.1 *NGUYÊN LÍ HO T ĐÔNG:Ạ Nh n phím kh i đ ng , ấ ở ộ xe xoay vòng, sau đó dò tìm đ i t ng ng i, khi c m bi n PIR phátố ượ ườ ả ế hi n có s hi n di n ng i , xe d ng xoay, ch y v h ng có ng i di chuy n đó, trong lúcệ ự ệ ệ ườ ừ ạ ề ướ ườ ể ch y đ n xe đo kho ng cách và ch y v i t c đ h p lí h n, bám theo đ i t ng.ạ ế ả ạ ớ ố ộ ợ ơ ố ượ 2.2/ S Đ M CH T NG KH I:Ơ Ồ Ạ Ừ Ố A/ M ch x lí trung tâm:ạ ử - Nhi m v : nh n tín hi u t các c m bi n PIR, siêu âm, tín hi u vô tuy n , xu t tín hi uệ ụ ậ ệ ừ ả ế ệ ế ấ ệ đi u khi n ra m ch cách li công su t lái 2 đ ng c ho t đ ng theo ý đ l p trình.ề ể ạ ấ ộ ơ ạ ộ ồ ậ - S đ m ch nh trên, chân 1:m ch reset tích c c m c th p, dùng port D, portB nh n giá trơ ồ ạ ư ạ ự ứ ấ ậ ị đi u khi n, port C xu t tín hi u đi u khi n v i:ề ể ấ ệ ề ể ớ o ROLE_L :đ a ch C0, đ o chi u đ ng c trái.ị ỉ ả ề ộ ơ o PWM_L:đ a ch C1, c p xung đi u khi n t c đ ng c trái.ị ỉ ấ ề ể ố ộ ơ o ROLE_R:đ a ch C3, đ o chi u đ ng c ph i.ị ỉ ả ề ộ ơ ả o PWM_R:đ a ch C2, c p xung đi u khi n t c đ ng c ph i.ị ỉ ấ ề ể ố ộ ơ ả - Vi đi u khi n là PIC 16F877A ,nh ph n gi i thi u ph n lí thuy tề ể ư ầ ớ ệ ầ ế 16F877A có h trỗ ợ module PWM 2 kênh thu n ti n trong đi u khi n t c đ đ ng c (do xe ch y nhi u ch đậ ệ ề ể ố ộ ộ ơ ạ ở ề ế ộ nên c n nhi u t c đ ),đ ng th i PIC có h tr ph n m m chuyên l p trình riêng cho nó nênầ ề ố ộ ồ ờ ỗ ợ ầ ề ậ t ng đ i d ti p c n l p trình h n.ươ ố ễ ế ậ ậ ơ B/ M ch vô tuy n : ạ ế - Nhi m v : truy n tín hi u đi u khi n c a ng i đi u khi n đ n xe,g m 4 kênh :A,B,C,Dệ ụ ề ệ ề ể ủ ườ ề ể ế ồ ,tích c c m c 1, có duy trì:ự ứ + Kênh A: kh i đ ng.ở ộ + Kênh B: d ng.ừ + Kênh C. +Kênh D. C/ M ch công cu t cách li:ạ ấ +Nguyên Lí Đi u R ng Xung:ề ộ -T i 2 chân PWM_R, PWM_L vi đi u khi n c p ra xung có th đi u ch nh T_ON, T_OFF, giáạ ề ể ấ ể ề ỉ tr thay đ i tùy theo yêu c u v n hành ng i l p trìnhị ổ ầ ậ ườ ậ -Chu kỳ xung ra đ c tính nh sau:ượ ư (1/20000000)*4*16*256= 819.2 us ho cặ 1.2 KHz Dùng th ch anh 20MHz ạ Chu kì xung đ c chia làm 256 m uượ ẫ nh m ch trên m ch tích c c m c”0” nên khi mu n tăng t c đ đ ng c ta tăng giá trư ạ ạ ự ở ứ ố ố ộ ộ ơ ị T_ON qua câu l nh ví d PWM_L =PWM1, PWM_R =PWM2ệ ụ set_pwm1_duty(255-value); -Giá tr T_ON = 255-value.ị -2 chân ROLE_R,ROLE_L dùng đóng đi n cho cu n dây r le trái và ph i , đ o ti p đi m =>ệ ộ ơ ả ả ế ể đ o chi u dòng đi n qua đ ng c ,đ ng c quay đ o chi u.ả ề ệ ộ ơ ộ ơ ả ề * Chú ý : m i đ ng c có s đáp ng dòng không gi ng nhau nên khi xét giá tr ta căn c th cỗ ộ ơ ự ứ ố ị ứ ự t m i đ ng c .ế ỗ ộ ơ D/ M ch Ngu n:ạ ồ Ngu n DC 12v c p t acqui khô, dùng 2 acqui c p cho m ch vi đi u khi n và công su t.ồ ấ ừ ấ ạ ề ể ấ 2.3L U Đ GI I THU TƯ Ồ Ả Ậ A/ L u Đ Gi i Thu t Ch ng Trình Mainư ồ ả ậ ươ +Gi i thích l u đ :ả ư ồ Khi xe b t ngu n lên, xe d ng t i ch ,khi có tín hi u kh i đ ng bit RF_A=1, xe ch y quayậ ồ ừ ạ ổ ệ ở ộ ạ vòng tìm ng i, n u phát hi n có ng i xe d ng l i , đo kho ng cách gi a ng i và xe, n uườ ế ệ ườ ừ ạ ả ữ ườ ế có ng i và kho ng cách ng i và xe <=1m, xe ch y bám theo, so sánh giá tr kho ng cách,soườ ả ườ ạ ị ả sánh và đi u khi n.ề ể B L u Đ Ch ng Trình Đo Kho ng Cách,So Sánh Và Đi u Khi n: ư ồ ươ ả ề ể Gi i thích l u đ :ả ư ồ SRF05 ho t đ ng, đo giá tr nh n v ,so sánh kho ng cách, tùy theo giá tr kho ng cách xeạ ộ ị ậ ề ả ị ả ch y v i nhi u t c đ đ bám theo đ ng th i kích xung đo giá tr m i, kho ng 2s xe ki m traạ ớ ề ố ộ ể ồ ờ ị ớ ả ể có còn ng i hay không ,ch y quét l i.ườ ạ ạ CH NG 3: ĐÁNH GIÁ VÀ K T LU NƯƠ Ế Ậ 3.1 K t Lu n:ế ậ V i s n l c h t mình và s h ng d n t n tình giáo viên h ng d n đ tài em đã hoànớ ự ổ ự ế ự ướ ẫ ậ ướ ẫ ề thành và k t qu là hình 3.1ế ả ế Hình 3.1 Đ tài tuy đã hoàn thành nh ng do kinh nghi m cũng nh ki n th c h n ch kính mong quýề ư ệ ư ế ứ ạ ế th y cô đóng góp cho em nh ng h ng gi i quy t thêm.ầ ữ ướ ả ế Em t nh n xét đ tài có nh ng u khuy t đi m sau:ự ậ ề ữ ư ế ể u đi m: Ư ể Xe kh i đ ng và d ng t xa,r t thu n ti n,trong lúc xe bám theo đ i t ng ta có th l p trìnhở ộ ừ ừ ấ ậ ệ ố ượ ể ậ nh ng kho ng cách bám h p lí, xe trang b ngu n đi u khi n và ngu n công su t r i nên ho tữ ả ợ ị ồ ể ể ồ ấ ờ ạ đ ng t ng đ i n đ nh,khi xe b l c đ i t ng t tìm l i ,bám theo.ộ ươ ố ổ ị ị ạ ố ượ ự ạ Khuy t đi m: ế ể C m bi n nh n ng i PIR ho t đ ng không n đ nh , d nhi u nên d xác đ nh sai đ iả ế ậ ườ ạ ộ ổ ị ễ ễ ễ ị ố t ng bám theo, d l c đ i t ng, t c đ xe bám theo không cao.ượ ễ ạ ố ượ ố ộ 3.2 TÀI LI U THAM KH O:Ệ Ả 1 GS Ph m Văn t__K Thu t L p Trình C.Năm2006 NXB Giao Thông V n T i.ạ Ấ ỹ ậ ậ ậ ả 2 Các website h tr :ỗ ợ -www.picvietnam.com(di n đàn c m bi n…)ễ ả ế -www.diendandientu.vn(di n đàn PIC)ễ -www.google.com.vn -www.alldatasheet.com 3.3 PH L C CH NG TRÌNH:Ụ Ụ ƯƠ CH NG TRÌNH ĐI U KHI N HOÀN THI N:ƯƠ Ề Ể Ệ #include #include __CONFIG(HS & WDTDIS); //khai báo input/output cho xe #define Relay_l RC0 #define Relay_r RC3 #define Pwm_l RC1 #define Pwm_r RC2 #define Cbnhan RB7 #define Trigger RB1 #define echo RB0 #define Rf_A RD0 #define Rf_B RD1 #define Rf_C RD2 #define Rf_D RD3 #define Speed_ch1 CCPR1L #define Speed_ch2 CCPR2L //------------------------------------------------ typedef unsigned int uint; typedef unsigned char uchar; //khai báo các bi n nhế ớ static uchar DistanceL, DistanceH, remember, times, sub; //------------------------------------------------ void delay (unsigned int dem) { //Delay khoang 'dem' mili giay unsigned int z; while (dem) { z = 230; while (z>0) z--; dem--; } } //***************************************** void delay10us (void) { uchar z; z = 20; while (z>0) z--; } //khai báo ng t ngoài*************************ắ void Intr_Init (void) { GIE = 1; INTE = 1; INTEDG = 1; //c nh lên************************ạ } //khai báo timer1******************************* void Timer1_Init (void) { T1CON = 0x25; //enable timer 1, prescaler = 4 } //ch ng trình ng t ngoài đo kho ng cách**************ươ ắ ả void interrupt distance (void) { if (INTE && INTF) { INTF = 0; if (INTEDG) { INTEDG = 0; //ng t c nh xu ngắ ạ ố TMR1H = 0; TMR1L = 0; } else { INTEDG = 1; //ng t c nh lênắ ạ DistanceL = TMR1L; //đ c giá tr l u vào BYTE_Lọ ị ư DistanceH = TMR1H; //đ c giá tr l u vàoọ ị ư BYTE_H } } } //+++++++++ PWM +++++++++++++ void PWM_EN() {CCP1CON |= 0x0c;} void PWM_UN() {CCP1CON &= 0xf0;} //------------------------------------ void Init_pwm () { TRISC = 0x00; PR2 = 0xff; T2CON = 0x07; //prescaler = 16, enable timer2 TMR2 = 0x00; CCP1CON = 0x0f; /*PWM mode on CCP1 module*/ CCP2CON = 0x0f; /*PWM mode on CCP2 module*/ } ///các ch ng trình con***********************ươ void stop (void) { Speed_ch1 = 255; Speed_ch2 = 255; } //----------- stop_h ------------------------ void stop_h (void) { Relay_r = Backward; Relay_l = Backward; delay (8); Speed_ch1 = 255; Speed_ch2 = 255; } //------------ speed_slow-------------- void speed_slow (void) { Relay_l = 1; Relay_r = 1; Speed_ch1 = 240; //Motor right Speed_ch2 = 230; //Motor left } //------------ speed_1-------------- void speed_1 (void) { Relay_l = 1; Relay_r = 1; Speed_ch1 = 224; //Motor right Speed_ch2 = 200; //Motor left } //------------speed_2--------------- void speed_2 (void) { Relay_l = 1; Relay_r = 1; Speed_ch1 = 188; //Motor right Speed_ch2 = 140; //Motor left } //------------speed_3--------------- void speed_3 (void) { Relay_l = 1; Relay_r = 1; Speed_ch1 = 113; //Motor right Speed_ch2 = 60; //Motor left } //------------speed_4--------------- void speed_4 (void) { Relay_l = 1; Relay_r = 1; Speed_ch1 = 83; //Motor right Speed_ch2 = 20; //Motor left } //=================back_w============== void back_w (void) { Relay_l = 0; Relay_r = 0; Speed_ch1 = 246;; //Motor right Speed_ch2 = 244; //Motor left } //--------- Trigger pulse -------------- void trigger (void) { Trigger = 1; delay10us(); Trigger = 0; } //Ch ng Trình Chính*************************ươ void main (void) { stop(); Relay_l = 1; Relay_r = 1; TRISB1 = 0; //trigger TRISB0 = 1; //echo TRISB7 = 1; TRISC = 0x00; TRISD = 0xff; TRISA = 0x00; ADCON1 = 0x06; // config IN/OUT port Trigger = 0; Intr_Init(); Timer1_Init(); Init_pwm(); while (1) { stop(); while (Rf_A == 1) { Relay_l =1; Relay_r =0; Speed_ch1 = 245; &a mp;n bsp; //thay doi toc do quay Speed_ch2 = 245; &a mp;n bsp; // while (Cbnhan == 0) { delay (10); stop (); while (Rf_A == 1) { trigger(); delay(3); Relay_l = 1; Relay_r = 0; Speed_ch1 = 250; //thay doi toc do quay Speed_ch2 = 250; // while ((Cbnhan == 0)&&(Rf_A == 1)&&(DistanceH < 35)) { delay (10); stop(); for (sub = 0; sub < 150; sub++) { //thoi gian quet cam bien lan ke tiep trigger(); delay(5); if (DistanceH < 35) { trigger (); delay(5); if (DistanceH < 8) { //(15 = 50cm) back_w(); delay(20); } else if ((DistanceH >= 8) && (DistanceH < 14)) { stop (); delay(20); } else if ((DistanceH >= 14) && (DistanceH < 19)) { speed_slow(); delay(20); } else if ((DistanceH >= 19) && (DistanceH < 23)) { speed_1(); delay(20); } else if ((DistanceH >= 23) && (DistanceH < 26)) { speed_2(); delay(19); } else if ((DistanceH >= 26) && (DistanceH < 35)) { speed_3(); delay(18); } } else { stop(); delay(17); } } } } } } } }