Điều khiển camera bám theo đối tượng

Đại Học Quốc Gia Tp Hồ Chí Minh TRƯỜNG ĐẠI HỌC BÁCH KHOA LÝ THANH PHƯƠNG ĐỀ TÀI LUẬN VĂN THẠC SĨ Chuyên ngành : TỰ ĐỘNG HÓA LUẬN VĂN THẠC SĨ TP HỒ CHÍ MINH, tháng năm 2008 CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA ĐẠI HỌC QUỐC GIA TP HỒ CHÍ MINH Cán hướng dẫn khoa học : TS Trương Đình Châu Cán chấm nhận xét : Cán chấm nhận xét : Luận văn thạc sĩ bảo vệ HỘI ĐỒNG CHẤM BẢO VỆ LUẬN VĂN THẠC SĨ TRƯỜNG ĐẠI HỌC BÁCH KHOA, ngày tháng năm 2008 TRƯỜNG ĐẠI HỌC BÁCH KHOA CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM PHÒNG ĐÀO TẠO SĐH ĐỘC LẬP – TỰ DO – HẠNH PHÚC Tp HCM, ngày tháng năm 2008 NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên: Ngày, tháng, năm sinh: Chuyên ngành: LÝ THANH PHƯƠNG 06-11-1983 TỰ ĐỘNG HÓA Phái: Nam Nơi sinh: An Giang MSHV: 01506366 I- TÊN ĐỀ TÀI: ĐIỀU KHIỂN CAMERA BÁM THEO ĐỐI TƯỢNG II- NHIỆM VỤ VÀ NỘI DUNG: • Tìm hiểu, lựa chọn phương pháp bám theo đối tượng • Thi cơng mơ hình bệ xoay camera có tốc độ thay đổi • Triển khai hệ thống bám đối tượng cụ thể với mô hình làm III- NGÀY GIAO NHIỆM VỤ: 30-01-2008 (Ngày bắt đầu thực LV ghi Quyết định giao đề tài): IV- NGÀY HOÀN THÀNH NHIỆM VỤ: 25-6-2008 V- CÁN BỘ HƯỚNG DẪN: CÁN BỘ HƯỚNG DẪN (Học hàm, học vị, họ tên chữ ký) TS TRƯƠNG ĐÌNH CHÂU TS TRƯƠNG ĐÌNH CHÂU CN BỘ MƠN QL CHUYÊN NGÀNH TS NGUYỄN ĐỨC THÀNH Nội dung đề cương luận văn thạc sĩ Hội đồng chuyên ngành thơng qua Ngày tháng năm 2008 TRƯỞNG PHỊNG ĐT – SĐH TRƯỞNG KHOA QL NGÀNH Lời cảm ơn LỜI CẢM ƠN Trước tiên, xin chân thành cảm ơn quan tâm hỗ trợ, tạo điều kiện hết lòng động viên tinh thần lẫn vật chất thành viên gia đình suốt thời gian qua Đồng thời xin cảm ơn thầy Trương Đình Châu gợi mở, quan tâm tạo thuận lợi cho thân học viên suốt thời gian thực đề tài tốt nghiệp Bên cạnh xin chuyển lời cám ơn đến : • Thầy Nguyễn Ngọc Khai, người thầy mà học viên may mắn có dịp học hỏi suốt q trình học tập, thầy khơi gợi ứng dụng lý thú từ buổi đầu tìm hướng đề tài • Anh Nguyễn Khắc Nguyên, người hỗ trợ học viên thực mạch điều khiển hoạt động tin cậy ổn định để học viên thử nghiệm hoàn chỉnh hệ thống phần cứng tiến độ đặt • Anh Nguyễn Thiện Minh, người định hướng hỗ trợ số thành phần quan trọng có liên quan để học viên tiếp nối phát triển mở rộng phạm vi đề tài Ngoài ra, học viên xin gởi lời cám ơn đến tất thầy trực tiếp giảng dạy suốt khóa học, người bạn quan tâm, động viên giữ mối liên lạc tốt trình học tập rèn luyện vừa qua Đại học Bách Khoa Tp.Hồ Chí Minh, tháng năm 2008 Học viên thực Lý Thanh Phương Trang i Tóm tắt TĨM TẮT Cùng với phát triển không ngừng khoa học công nghệ, hầu hết công việc giản đơn mà không hẳn phải dùng đến nhân công thay máy móc đại đáng tin cậy Xu hướng thể rõ ràng hàng loạt máy móc, thiết bị đời có số khả sinh vật thực thụ hay nói cách khác chúng có trí thơng minh nhân tạo ln sẵn sàng thực xác nhiệm vụ người cần mà mỏi mệt, buồn chán hay tập trung Điều thực có ý nghĩa to lớn góp phần tăng thêm giá trị chất lượng sống Đề tài luận văn này, điều khiển camera bám theo đối tượng, khơng nằm ngồi ý nghĩa Đây dạng ứng dụng thú vị mà nhiều nhà khoa học đã, không ngừng nghiên cứu nhu cầu thực tiễn đa dạng phương pháp tiếp cận Báo cáo trình bày bước tình huống, khó khăn mà học viên đối mặt bao gồm cách giải suốt trình triển khai để đề tài đảm bảo yêu cầu, nhiệm vụ đặt Học viên sử dụng công cụ SharperCV ứng dụng đặc trưng Haar-like thuật toán tăng tốc AdaBoost nhằm đẩy nhanh q trình dị tìm khn mặt người giúp tạo tiền đề cho chức điều khiển bám hệ thống Ở bước kế tiếp, vấn đề điểu khiển bệ xoay mang camera bám theo khuôn mặt học viên thực theo hướng sử dụng kỹ thuật điều khiển mờ dựa thông số khoảng cách độ thay đổi khoảng cách đối tượng so với tâm khung ảnh Kết kiểm chứng mơ hình bệ xoay học viên cải tiến từ khung loại sản phẩm sẵn có thị trường Đề tài có tính thực nghiệm tập trung nghiên cứu giải khoảng thời gian hạn hẹp nên chắn không tránh khỏi sai sót mặt nội dung hình thức thể Mong q thầy bạn vui lịng thơng cảm góp ý thêm để học viên có hội học hỏi trao đổi thêm Trang ii Mục lục MỤC LỤC Chương : GIỚI THIỆU TỔNG QUAN 1.1 GIỚI THIỆU .1 1.2 ĐIỀU KHIỂN CAMERA PHÁT HIỆN VÀ BÁM MẶT NGƯỜI .2 1.2.1 Phát biểu toán 1.2.1.1 Phát mặt người .2 1.2.1.2 Bám theo mặt người 1.2.2 Lý thực đề tài : 1.2.3 Mục tiêu đề tài 1.2.4 Các vấn đề khó khăn 1.2.4.1 Các khó khăn phương pháp 1.2.4.2 Các khó khăn tốc độ thực thi phần cứng hệ thống 1.2.5 Nhiệm vụ luận văn 1.2.6 Phạm vi thực .6 1.2.7 Dự kiến lĩnh vực ứng dụng đề tài: Chương : CƠ SỞ LÝ THUYẾT 2.1 CÁC PHƯƠNG PHÁP PHÁT HIỆN (DỊ TÌM) KHN MẶT NGƯỜI 2.1.1 Phát dựa ảnh 2.1.2 Phát dựa dạng hình học 2.2 PHƯƠNG PHÁP ĐƯỢC SỬ DỤNG TRONG LUẬN VĂN 10 2.2.1 Các đặc trưng Haar-like 10 2.2.2 Thuật toán tăng tốc AdaBoost .13 Chương : XÂY DỰNG HỆ THỐNG 18 3.1 Ý TƯỞNG XÂY DỰNG HỆ THỐNG VÀ HOẠT ĐỘNG 18 3.2 TIẾN HÀNH XÂY DỰNG THUẬT TOÁN 18 3.2.1 Thuật tốn phát khn mặt 18 3.2.2 Thuật toán bám theo đối tượng 18 3.3 TIẾN HÀNH XÂY DỰNG PHẦN MỀM .21 3.3.1 Dùng thuật toán Haar-like dị tìm khn mặt ảnh 22 Trang iii Mục lục 3.3.2 Chương trình điều khiển bệ xoay mang camera 22 3.3.3 Hoạt động phần mềm 25 3.3.4 Giao diện chương trình phần mềm hệ thống 27 3.4 TIẾN HÀNH XÂY DỰNG PHẦN CỨNG PHỤC VỤ HỆ THỐNG 28 3.4.1 Camera 28 3.4.2 Card hình giao tiếp với máy tính 28 3.4.3 Bệ xoay mang camera 29 3.4.3.1 Bệ xoay trước cải tiến 29 3.4.3.2 Bệ xoay sau cải tiến 32 3.4.4 Mạch điều khiển motor 33 3.4.5 Chương trình vi xử lý nạp xuống mạch điều khiển 34 3.4.6 Hệ thống hoàn chỉnh sử dụng .37 3.4.7 Một số thông số hệ thống : 39 Chương : KẾT QUẢ – NHẬN XÉT VÀ PHÂN TÍCH 41 4.1 KẾT QUẢ THỬ NGHIỆM .41 4.2 NHẬN XÉT VÀ PHÂN TÍCH KẾT QUẢ ĐẠT ĐƯỢC 42 4.2.1 Nhận xét 42 4.2.1.1 Ưu điểm : .42 4.2.1.2 Nhược điểm : .42 4.2.2 Phân tích kết đạt hướng khắc phục 43 Chương : KẾT LUẬN 45 5.1 KẾT QUẢ ĐẠT ĐƯỢC VÀ Ý NGHĨA 45 5.2 HƯỚNG PHÁT TRIỂN 46 5.3 KẾT LUẬN .46 CƠNG TRÌNH HỌC VIÊN ĐÃ THỰC HIỆN 48 TÀI LIỆU THAM KHẢO VÀ TRÍCH DẪN 49 PHỤ LỤC 51 Trang iv Mục lục A Một số lệnh SharperCV xử dụng luận văn .51 B Chương trình vi xử lý nạp cho AVR 54 LÝ LỊCH TRÍCH NGANG 71 Trang v Danh sách hình Danh sách hình Hình 2.1a Các đặc trưng cạnh .10 Hình 2.1b Các đặc trưng đường 11 Hình 2.1c Các đặc trưng khoanh vùng 11 Hình 2.2 : Ảnh chia nhỏ tọa độ (x,y) 12 Hình 2.3 : Tổng giá trị pixel nằm vùng A .12 Hình 2.4 : Lược đồ AdaBoost .13 Hình 2.5 : Thuật toán học AdaBoost .14 Hình 2.6 : Dùng chuỗi cascade phát cửa sổ phù hợp 16 Hình 2.7 : Cấu trúc chuỗi cascade song song 17 Hình 3.1 : Lưu đồ thể trình bám đối tượng hệ thống 20 Hình 3.2 : Sơ đồ khối tổ chức phần mềm hệ thống 21 Hình 3.3 : Mơ tả hệ thống dị tìm khn mặt 22 Hình 3.4 : Mô tả kỹ thuật điều khiển bệ xoay camera bám đối tượng 24 Hình 3.6 : Mẫu camera sử hdụng luận văn 28 Hình 3.7 : Card đồ họa AverTV Cardbus dùng luận văn 29 Hình 3.8 : Hình dáng bệ xoay mang camera trước cải tiến 30 Hình 3.9 : Tháo rời bệ xoay trưóc cải tiến 30 Hình 3.10 : Hộc bệ xoay trước cải tiến 31 Hình 3.11 : Mơ tơ cơng tắc hành trình bệ xoay cũ 31 Hình 3.12 : Các mơ tơ DC có encoder dùng cho bệ xoay cải tiến 32 Hình 3.13 : Bệ xoay sau cải tiến 33 Hình 3.14 : Hình dạng tổng thể mạch điểu khiển mô tơ DC .34 Hình 3.15 : Sơ đồ mạch điều khiển motor bệ xoay mang camera 36 Hình 3.16 : Sơ đồ luận lý nạp cho IC GAL16V8 37 Hình 3.17 : Sơ đồ khối hệ thống hoàn chỉnh 37 Hình 3.18 : Hình ảnh tổng quan hệ thống hoàn chỉnh thực tế .38 Hình 3.19 : Sơ đồ khối hóa hệ thống thực tế .38 Hình 4.1 : Phát khoanh vùng mặt nhìn thẳng, xoay ±20 độ 41 Trang vi Danh sách bảng Danh sách bảng Bảng 2.1 : Thuật toán AdaBoost 15 Bảng 3.1 : Bộ luật điều khiển mờ .24 Bảng 3.2 : Các thông số mô tơ cần đặt .35 Bảng 3.3 : Các thành phần có liên quan đến mơ hình .39 Trang vii Lý Thanh Phương cpi rjeq cpi breq cpi breq cpi breq cpi breq cpi breq Phụ lục BH, 'W' do_weep BH, 'M' do_mode BH, 'S' do_sub BH, 'P' do_parm BH, 'E' do_echo BH, '?' do_help cmd_err:ldiw rjmp PC+3 do_help:ldiw rcall rjmp main ;Write parms? ; ;Set mode? ; ;Set sub command? ; ;Set parms? ; ;Echo mode? ; ;Help? ; Z,m_error*2 ;Syntax error Z,m_help*2 dp_str ;Help ; ; ; Change parameters, command regs or servo mode do_mode: ; Change servo mode rcall get_val breq cmd_err rcall init_servo rjmp main do_echo: ; Change echo mode rcall get_val breq cmd_err bmov _Flags,0, AL,0 rjmp main do_sub: ldiw rjmp ; Set subcommand reg Y, CtSub ds_set do_parm: ; Set parameters rcall get_val breq cmd_err cpi AL, N_PARM brcc cmd_err lsl AL mov YL, AL clr YH subiw Y, -Parms ds_set: rcall get_val brcs cmd_err brne ds_st ldi AL, 0x0a rcall xmit lddw A, Y+0 clr BL sbrc AH, dec BL rcall dp_dec ldi AL, ':' Trang 57 Lý Thanh Phương Phụ lục rcall xmit rcall get_line rcall get_val brcs cmd_err breq PC+5 ds_st: cli stdw Y+0, A sei rjmp main ; ; ; Load/Save parameters do_reep: ; Load parameters from EEPROM rcall get_val breq cmd_err cpi AL, (EEPROMEND+1)/N_PARM/2 brcc cmd_err cli rcall load_parms sei rjmp main do_weep: ; Save parameters into EEPROM rcall get_val breq cmd_err cpi AL, (EEPROMEND+1)/N_PARM/2 brcc cmd_err rcall get_eeadr sbic EECR, EEWE rjmp PC-1 out EEAR, BH inc BH ld AL, Y+ out EEDR, AL cli sbi EECR, EEMWE sbi EECR, EEWE sei dec AH brne PC-11 rjmp main load_parms: rcall get_eeadr out EEAR, BH inc BH sbi EECR, EERE in AL, EEDR st Y+, AL dec AH brne PC-6 ret get_eeadr: ldi clr subi AH, N_PARM*2 BH AL, Trang 58 Lý Thanh Phương brcs add rjmp ldiw ret Phụ lục PC+3 BH, AH PC-3 Y, Parms ; ; ; Show location counter do_loc: ldi AL, 0x0a rcall xmit dp_p: ldi AL, 0x0d ;Show position counter rcall xmit ; cli ; movw AL, _PosL ; mov BL, _PosX ; sei ; mov T0H, AL ; rcall dp_dec ; ldi AL, ' ' ; rcall xmit ;/ rcall receive ;Break if any key was pressed rjne main ;/ cp T0H, _PosL ;Continue if not changed breq PC-4 ;/ rjmp dp_p ; ; ; Change position command reg immediataly do_jump: rcall get_val rjeq cmd_err ldiw Z, CtPos ;Set position command reg cli ; stdw Z+0, A ; std Z+2, BL ; sei ;/ rjmp main ; ; ; Go at trapezoidal/rectanguler velocity profile do_go: ldiw Z, CtPos ;Z -> Position command reg lddw T4, Z+0 ;T6L:T4 = start posision ldd T6L, Z+2 ;/ mov T0H, T4L ;r3:r0 = commanded posision mov T2L, T4H ; mov T2H, T6L ;/ rcall get_val ;sub command rjeq cmd_err ;/ cpi AL, ;G0? breq dg_0 ;/ cpi AL, ;G1? breq dg_1 ;/ rjmp cmd_err Trang 59 Lý Thanh Phương Phụ lục ; Rectanguler velocity profile dg_1: rcall get_val ;BL:AL = target posision rjeq cmd_err ;/ pushw A push BL rcall get_val ;CH:BH = velocity mov BH, AL ; mov CL, AH ; mov CH, _0 ;/ pop BL popw A rjeq cmd_err ;/ cpw A, T4 ;T = direction cpc BL, T6L ; clt ; brge PC+2 ; set ;/ clr r0 ; rcall dg_add ; -Constant velocity loop brlt PC-1 ; rjmp dg_end ;/ ; Trapezoidal velocity profile dg_0: rcall get_val ;BL:AL = target posision rjeq cmd_err ;/ cpw A, T4 ;T = direction cpc BL, T6L ; clt ; brge PC+2 ; set ;/ clr r0 ; clr BH ;CL:BH = start velocity clrw C ;/ dg_ul: lds DL, MvAcc+0 add BH, DL lds DL, MvAcc+1 ; adc CL, DL adc CH, _0 rcall dg_add brge dg_end movw DL, AL distance mov EL, BL sub DL, T4L sbc DH, T4H sbc EL, T6L asr EL ; rorw D ; addw D, T4 ; adc EL, T6L brts PC+6 ; cp T0H, DL cpc T2L, DH cpc T2H, EL brge dg_de ; rjmp PC+5 ; ; -Up ramp loop ;Increace velocity ; ;/ ;Check current posisiton has passed half of ;If passed, enter to down ramp ; ; ; ; ; ; ; Trang 60 Lý Thanh Phương cp cpc cpc brge ldsw cp cpc cpc brcs movw mov sub sbc sbc addw adc dg_cl: brts cp cpc cpc brge rjmp cp cpc cpc brlt DL, T0H DH, T2L EL, T2H dg_de D, MvSpd BH, DL CL, DH CH, _0 dg_ul DL, T4L EL, T6L DL, T0H DH, T2L EL, T2H D, A EL, BL rcall PC+6 T0H, DL T2L, DH T2H, EL dg_de dg_cl DL, T0H DH, T2L EL, T2H dg_cl Phụ lục ; ; ; ;/ ;Has current velocity reached P6? ;If reached, enter constant velocity mode ; ; ;/ ;Calcurate down ramp point ; ; ; ; ; ;/ EL:DL = s.p - c.p + t.p dg_add ; -Constant velocity loop dg_de: rcall dg_add dg_dl: lds DL, MvAcc+0 ; -Down ramp loop sub BH, DL ;Decrese velocity lds DL, MvAcc+1 ; sbc CL, DL ; sbc CH, _0 ;/ brcs dg_end rcall dg_add brlt dg_dl dg_end: cli stdw Z+0, A std Z+2, BL dge_lp: cli cpw _Pos, A cpc _PosX, BL sei breq PC+5 push AL rcall receive pop AL breq dge_lp rjmp main dg_add: cbr _Flags, bit7 push AL ; rcall receive ; End of action ; Wait until position stabled ;Wait for 1kHz time interval ; Trang 61 Lý Thanh Phương pop brne sbrs rjmp sbrc rjmp sbrc rjmp brts add adc adc adc cp cpc cpc brge rjmp sub sbc sbc sbc cp cpc cpc brge cli std stdw sei ses dga_ov: AL dga_stop _Flags, PC-5 _Flags, dg_add _Flags, dg_add PC+10 T0L, BH T0H, CL T2L, CH T2H, CH T0H, AL T2L, AH T2H, BL dga_ov PC+9 T0L, BH T0H, CL T2L, CH T2H, CH AL, T0H AH, T2L BL, T2H dga_ov ret dga_stop: pop pop rjmp AL AL main Phụ lục ; ; ; ;/ ;Skip if torque limit has being occured ; ; ;/ ;Increase commanded point by current velocity ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ;/ Z+0, T0H Z+1, T2 ; ; ; Initialize servo system ; ;Call: AL = servo mode init_servo: cli sts ldiw st cpi brne clrw clr sei cbi cbi sbi ret Mode, AL ;Set servo mode Y, CtPos ;Clear cmmand regs and servo operators Y+, _0 ; YL, low(CtPos+11); PC-2 ;/ _Pos ;Clear position counter _PosX ;/ PORTB, PORTB, PORTB, ;Error LED off ;Torque LED off ;Ready LED on Trang 62 Lý Thanh Phương Phụ lục ; ; ; 83kHz Position capture and servo operation interrupt background: push T0L pushw Z in T0L, SREG ;Save flags mov ZL, _PvEnc ;ZL[1:0] = previous A/B signal in _PvEnc, PIND ;Sample A/B signal into _PvEnc[1:0] swap _PvEnc ;/ ldi ZH, ;Convert it to sequencial number sbrc _PvEnc, ; eor _PvEnc, ZH ;/ sub ZL, _PvEnc ;Decode motion andi ZL, ;/ breq enc_zr ;-> Not moved cpi ZL, ; breq enc_rev ;-> -1 count cpi ZL, ; breq enc_fwd ;-> +1 count mov ZL, _PvDir ;-> Missing code recovery: mov ZH, _PvDir ; double count for previous direction lsl ZL ; asr ZH ;/ rjmp enc_add enc_rev:ldiw Z, -1 rjmp PC+3 enc_fwd:ldiw Z, mov _PvDir, ZL enc_add:addw _Pos, Z adc _PosX, ZH enc_zr: dec _CtDiv ;Decrement 1/83 divider rjne bgnd_exit ;If not overflow, exit interrupt routine ; End of 83 kHz position captureing process Follows are 1kHz servo ; operation It will be interrupted itself, but will not be re-entered ldi mov sei pushw pushw pushw pushw pushw push pushw ldiw ZL, 83 _CtDiv, ZL ;Re-initialize 1/83 divider ;/ ;Enable interrupts T0 T2 A B C DL Y Y, Parms sbr _Flags, bit7 ; 1kHz inerrupt flag lddw cli subw stdw sei T2, Y+iPvPos ; T2, _Pos ; Y+iPvPos, _Pos ; ;Detect velocity ;Work area base pointer ; Trang 63 Lý Thanh Phương Phụ lục negw T2 ldd cpi breq lddw cpi breq cpi rjeq rjmp AL, Y+iMode ;Branch by servo mode AL, ;Mode3? tap_position ;/ T0, Y+iCtSub ;Get sub command AL, ;Mode2? tap_velocity ;/ AL, ;Mode1? tap_torque ;/ tap_voltage ;Mode0 tap_position: cli lddw T0, ldd BH, subw T0, sbc BH, sei lddw clr cpw cpc brge negw com cpw cpc brge movw ;/ T2 = velocity ;Get position error (= velocity command) Y+iCtPos ; Y+iCtPos+2 ; _Pos ; _PosX ; ;BL:T0 = position error A, Y+iLimSpd BL ; T0, A ; BH, BL PC+10 ; A ; BL ; T0, A ; BH, BL PC+2 ; T0L, AL ;Velocity limit (P0) ; ; ;T0 = velocity command tap_velocity: movw AL, T2L ;Velocity loop gain (P1) lddw B, Y+iGaSpd ; rcall muls1616 ;B = scaled velocity subw T0, B ;T0 = velocity error movw AL, T0L lddw B, Y+iGaTqP ; rcall muls1616 movw ZL, BL ;Velocity error P-gain (P2) ; ;Z = P term; lddw A, Y+iPvInt ;Velocity error I-gain (P3) lddw B, Y+iGaTqI ; rcall muls1616 ; addw Z, B ;Z += I term; cbr lddw cpw brlt movw sbr neg com cpw brge _Flags, bit6+bit5;Torque limit (P4) B, Y+iLimTrq ; Z, B ; PC+3 ; ZL, BL ; _Flags, bit6 ; BL ; BH ; Z, B ; PC+3 ; Trang 64 Lý Thanh Phương Phụ lục movw sbr ZL, BL _Flags, bit5 tst brmi sbrc rjmp rjmp sbrc rjmp lddw addw stdw T0H PC+4 _Flags, PC+10 PC+3 _Flags, PC+7 A, Y+iPvInt A, T0 Y+iPvInt, A mov andi limitter lddw breq sbi addiw rjmp cbi subiw brcs stdw ldiw cpw brcc movw ; ;/ ;PvInt += T0, with anti-windup ; ; ; ; ; ; ; ; ;/ AL, _Flags ;Check torque limiter timer AL, bit6+bit5 ; OvTmr is increased by when torque A, Y+iOvTmr PC+5 PORTB, A, PC+5 PORTB, A, PC+8 Y+iOvTmr, A B, 3000 A, B servo_error T0L, ZL ; works, decreased by when no torque limit ; When the value reaches 3000, servo error ; will beoccured ; ; ; ; ; ; ; ; ;/ ;T0 = torque command tap_torque: movw AL, T2L ;EG compensation (P5) lddw B, Y+iGaEG ; rcall muls1616 ; addw T0, B ;T0 = voltage command tap_voltage: ldiw A, 240 cpw T0, A brge PC+6 ldiw A, -240 cpw T0, A brge PC+2 movw T0L, AL asrw ldi adc ldi sub out out T0 AL, 128 AL, T0L AH, 128 AH, T0L OCR1AL, AL OCR1BL, AH popw pop popw Y DL C ;Clip output voltage between -240 and +240 ; Limit minimum duty ratio to 1/16 for bootstrap ; type FET driver ; ; ; ;T0 = PWM command ;Set PWM register (OCR1A and ~OCR1B) ; ; ; ; ; ;/ Trang 65 Lý Thanh Phương popw popw popw popw bgnd_exit: out popw pop reti Phụ lục B A T2 T0 SREG, T0L Z T0L ;End of encoder capture and servo operation ;Restore flags servo_error: clr AL ;Enter Mode0 rcall init_servo ;/ sbi PORTB, ;Error LED on clrw T0 ;Output off rjmp tap_voltage ; ; ; 16bit * 16bit signed multiply ; ; Multiplyer: A(signed int) ; Multiplicand: B(unsigned, 8.8 fraction) ; Result: B(signed int) ; Clk: 181(max) muls1616: clt tst brpl set negw AH PC+6 A subw ldi brcc addw rorw rorw dec brne C, C ; clear high 16bit DL, 17 ; DL = loop count PC+3 ; calcurating loop C, A ; C ; B ; DL ; if ( DL > 0) PC-8 ; continue loop; mov mov BL, BH BH, CL brtc negw ret PC+5 B ; Negate the result if multiplyer was negative ; ; ; Input a command line into LineBuf get_line: ldiw ldi rl_lp: breq st X,LineBuf BH,0 rcall PC-1 X,AL receive Trang 66 Lý Thanh Phương cpi AL,0x0d brne PC+4 ldiw X,LineBuf rjmp echo cpi AL,0x08 brne PC+7 cpi BH,0 breq rl_lp rcall echo sbiw XL,1 dec BH rjmp rl_lp cpi AL,' ' brcs rl_lp cpi BH,20-1 breq rl_lp rcall echo adiw XL,1 inc BH rjmp rl_lp Phụ lục ; CR ; BS ; SP ; ; ; Send ROM string ; ; Call: Z = top of the string (ASCIZ) ; Ret: Z = next string dp_str: lpm AL, Z+ tst AL brne PC+2 ret rcall xmit rjmp dp_str ; ; ; Get value of decimal string ; ; Call: X -> ASCII string ; Ret: X = updated ; if C=1: error ; elsif Z=1: end of line, value=0 ; else: BL:AL = 24bit value ; ; Positive: "300" ; Negative: "-125000" get_val: clt clr clr clr ld cpi brcs breq cpi brne set AL AH BL BH,X+ BH,' ' gd_n PC-3 BH,'-' PC+3 Trang 67 Lý Thanh Phương gd_l: ld cpi brcs subi brcs cpi brcc ldi ldi sub lsr ror ror ror brcc add dec brne add adc adc rjmp gd_x: sec sez ret gd_e: sbiw brtc com com com subi sbci sbci clc ret gd_n: sbiw clc sez ret Phụ lục BH,X+ BH,' '+1 gd_e BH,'0' gd_x BH,10 gd_x CL, 25 CH, 10 r0, r0 r0 BL AH AL PC+2 r0, CH CL PC-7 AL, BH AH, _0 BL, _0 gd_l XL,1 PC+7 AL AH BL AL,-1 AH,-1 BL,-1 XL,1 ; ; ; Display a value in decimal string ; ; Call: BL:A = 24bit signed value to be displayed ; Ret: BL:A = broken dp_dec: sbrs rjmp com com com adc adc adc ldi clr inc ldi CH,' ' BL, PC+8 AL AH BL AL,_0 AH,_0 BL,_0 CH,'-' T0L ;digit counter T0L ; decimal string generating loop Trang 68 Lý Thanh Phương clr BH ldi CL,24 lslw A rolw B cpi BH,10 brcs PC+3 subi BH,10 inc AL dec CL brne PC-9 addi BH,'0' push BH cp AL,_0 cpc AH,_0 cpc BL,_0 brne PC-18 mov AL, CH rcall xmit pop AL rcall xmit other display device dec T0L brne PC-3 ret Phụ lục ;var1 /= 10; ; ; ; ; ; ; ; ; ;/ ;Push the remander (a decimal digit) ;/ ;if(var1 =! 0) ; continue digit loop; ; ;/ ;Sign ;/ ;Transmit decimal string ;< Put a char to memory, console or any ; ;/ ; ; ; Serial I/O driver ; Transmit AL echo: sbrs _Flags, ret xmit: sbis UCSRA, UDRE rjmp PC-1 out UDR, AL ret receive:; Receive a char into AL (ZR=no data) push AH pushw Y ldiw Y, RxBuf cli ldd AH, Y+0 ldd AL, Y+1 cp AH, AL breq PC+8 add YL, AH ldd AL, Y+2 sub YL, AH inc AH andi AH, 15 std Y+0, AH clz sei popw Y pop AH ret rxint: ;USART0 Rx ready Trang 69 Lý Thanh Phương push in push in cbi sei pushw pushw ldiw ldd ldd inc andi cp breq std dec andi add std popw popw pop out pop cli sbi reti Phụ lục AL AL, SREG BL BL, UDR UCSRB, RXCIE A Y Y, RxBuf AL, Y+0 AH, Y+1 AH AH, 15 AH, AL PC+6 Y+1, AH AH AH, 15 YL, AH Y+2, BL Y A BL SREG, AL AL UCSRB, RXCIE ; ; ; Strings m_prompt: db 13,10, "%", m_error: db 10, "???", m_start: db 13,10, "SMC3 - Servo Motor Controller (?:help)", 13,10, m_help: db 13,10, "m - servo mode",13,10, "l - show position",13,10, "e{0|1} - echo mode",13,10, "s - sub cmd",13,10, "j - position cmd",13,10, "g0 - G0 cmd",13,10, "g1 - G1 cmd",13,10, "p [] - examine/change parms",13,10, "w save parms",13,10, "r - load parms", Trang 70 Lý Thanh Phương Lý lịch trích ngang LÝ LỊCH TRÍCH NGANG _ Họ tên : LÝ THANH PHƯƠNG Phái : Nam Ngày sinh : 06 – 11 – 1983 Nơi sinh : thị xã Châu Đốc, tỉnh An Giang Địa liên lạc : 16 Quang Trung, P Châu Phú B, Tx Châu Đốc, An Giang Địa e-mail : phuongly_ag@yahoo.com QUÁ TRÌNH ĐÀO TẠO 9/2001 – 4/2006 : Sinh viên khoa Điện – Điện tử, chuyên ngành Điều khiển Tự động, hệ qui Luận văn tốt nghiệp : "Nghiên cứu - ứng dụng hệ điều khiển phân tán SIMATIC PCS – 7" điểm : 8.6 Tổng điểm tích lũy : 7.45 9/2006 – đến : Học viên cao học khoa Điện – Điện tử, chuyên ngành Tự động hóa, hệ qui Trang 71 ... ĐIỀU KHIỂN CAMERA BÁM THEO ĐỐI TƯỢNG II- NHIỆM VỤ VÀ NỘI DUNG: • Tìm hiểu, lựa chọn phương pháp bám theo đối tượng • Thi cơng mơ hình bệ xoay camera có tốc độ thay đổi • Triển khai hệ thống bám. .. người, bám theo mặt người, nhận biết biểu cảm thể khuôn mặt người…) 1.2.1.2 Bám theo mặt người Với kết phát mặt người bước trên, từ điều khiển camera bám theo mặt người khung hình cho camera xoay theo. .. trạng thái Máy tính xuất lệnh điều khiển bệ xoay mang camera cho ảnh đối tượng cần bám đưa lân cận tâm khung hình camera Phương pháp bám đối tượng vùng lân cận theo học viên có ưu điểm đơn giản