Đồ án Robot di dộng bám theo vật

ục có tên “otherlibs”, sử dụng chúng rất dễ gặp lỗi. Vì rằng HighGUI chứa các thư viện vào ra cơ bản , bạn sẽ muốn chắc chắn hơn, đừng bỏ sót nó.Nó cũng chứa đựng nhiều cửa sổ đa chức năng. CVCAM là thư viện chứa các hàm truy nhập video thông qua DirectX trên môi trường Window 32 bit. Tuy nhiên, HighGUI cũng có các giao diện video.Trong bài báo này, tôi chỉ xem xét HighGUI. Chúng đơn giản, làm việc trên nhiều môi trường. Nếu bạn sử dụng Window XP hoặc 2000, bạn có thể làm tăng hiểu quả của nó bằng cách chuyển qua giao diện CVCAM, nhưng học OpenCV sẽ đơn giản hơn khi dùng HighGUI. Cài đặt OpenCV Cài đặt cơ bản OpenCV cho Linux và MacOSX được đóng gói như một gói mã nguồn lưu trữ. Bạn sẽ phải xây dựng cả thư viện tĩnh và cả những đối tượng chia sẻ (shared-object). Bạn cần xây dựng RPM trước tiên và cài đặt từ nó, hoặc biên dịch và cài đặt nó ngay.Cấu trúc cho cả hai được nằm trong INSTALL. Với Window ta khi cài đặt OpenCV,nó sẽ cop file OpenCV vào thư mục mà bạn chọn.Cách thức lựa chọn trong đường dẫn hệ thống của bạn chứa mã nhị phân OpenCV, đăng kí một vài bộ lọc DirectX.Mặc định nó cài đặt đến C:/Program Files/OpenCV/ Những yêu cầu khi cài đặt trong windows Đối với người dùng xài Windows, OpenCV rất dễ dàng cài đặtvà luôn hoạt động. nhưng một chút cải tiến nhỏ sẽ cho bạn kết quả tốt hơn. Sau đây là một vài đề nghị. OpenCV chỉ là một công cụ hỗ trợ cho người lập trình không phải là một chương trình bạn có thể lưu giữ nó ở một nơi nào đó khác thư mục Progam files. Nếu bạn thích cài vào một nơi khác hãy chọn thư mục đó trước khi nó hỏi. Tôi đề nghị bạn cũng quyết định trước khi cài đặt – bạn muốn windows tìm kiếm những files dll của OpenCV như thế nào. Bạn có thể chỉnh sửa đường dẫn của hệ thống của bạn thay vì của hệ thống, hoặc bạn có thể di chuyển chúng sau khi cài đặt từ thư mục “bin” của OpenCV bằng thư mục gốc của bạn. Nếu bạn muốn di chuyển các files dll nhưng bạn không chắc thư mục gốc của bạn nằm ở đâu, bạn có thể lưu chúng bằng cách chạy tiện ích Sysinfo có sẵn ở địa chỉ www.cognotics.com/utilities. Nếu bạn thích chỉnh đường dẫn hơn di chuyển các file dll, bạn có thể cài đặt bằng cách lựa chọn các check box “ Add bin directory to path”. Sau khi cài đặt Thư mục OpenCV có chứa và thư mục con. Thư mục “docs” có chứa các tài liệu html cho tất cả các hàm và kiểu dữ liệu. từ các tài liệu này bạn có thể làm các ví dụ, bạn cũng có thể duyệt (browse) thư mục “samples”. Bạn cũng cần đến những file header khi biên dịch chương trình nó có ở trong các module của OpenCv. Mặc dù bạn không cần làm việc này. Tôi thích tập hợp chúng lại với nhau vào trong một thư mục. Trên cả Linux và Windows, bạn có thể xác định vị trí của những header bằng cách tìm kiếm thư mục cài đặt và các thư mục con của nó với các file có đuôi mở rộng là *.h, *.hpp. sẽ có rất nhiều file được tìm thấy. bạn sẽ không cần tất cả các file. Header cho tất cả các modules ngoại trừ HighGUI đã được đính sẵn trong mỗi module. Bạn có thể bỏ các header trong thư mục “src” cho nhưng module này. Với HighGUI bạn sẽ cần đến file highgui.h được lưu ở otherlibs/highgui. Lập trình với OpenCV: vài điều cơ bản Những điều cần biết về Header và Libraries Hầu hết chương trình OpenCV cần cv.h và highgui.h.Sau đó để nhận dạng mặt chúng ta cần thêm cvaux.h. Phần còn lại của file header được thêm vào bởi những header cao nhất ( high-level headers). Nếu bạn để các file header trong các thư mục khác nhau( cài đặt mặc định), hãy chắc chắn rằng đường dẫn của trình biên dịch có các thư mục đó. Nếu bạn gom chúng lại một thư mục hãy chắc chắn rằng nó trùng với đường dẫn của trình biên dịch của bạn. Figure 1:Ví dụ chương trình đọc một ảnh từ file và ghi nó vào một file khác với dịnh dạng khác Mối liên kết của bạn sẽ cần cả hai thư viện và tên của thư viện tĩnh để sử dụng. thư viện tĩnh bạn cần liên kết đến file cxcore.lib, cv.lib, và highgui.lib. và sau đó bạn cần thêm file cvaux.lib để nhận dạng gương mặt. tất cả các file này đều nằm trong thư mục “lib”. Đọc và ghi ảnh Xuất nhập ảnh rất dễ dàng với OpenCV/ hình 1 chỉ cho chúng ta thấy một chương trình hoàn chỉnh để đọc một ảnh và ghi ảnh đó sang một file thứ hai trong một định dạng khác. Để đọc một file ảnh đơn giản ta chỉ cần gọi cvloadImage(), trong ví dụ nó nằm ở dòng thứ 14. OpenCV hỗ trợ hầu hết các định dạng ảnh phổ biến như JPEG, PNG và BMP. Bạn không cần phải cung cấp thông tin của định dạng ảnh. cvLoadImage xác định định dạng file bằng cách đọc các file header. Để ghi một hình ảnh ra file, gọi hàm cvSaveImage(). Hàm này quyết định định dạng file nào sẽ được dùng từ đuôi mở rộng. trong ví dụ này đuôi mở rộng là “PNG”. Vì vậy nó sẽ ghi dữ liệu ảnh dưới định dạng PNG. Cả hai hàm cvLoadImage() và cvSaveImage() đều nằm trong HighGUI module. Khi bạn hoàn thành việc sử dụng nhập ảnh từ cvLoadImage(), bạn cần thả free bằng cách gọi hàm cvReleaseImage(), như ở dòng 29 hàm này sẽ lấy địa chỉ của một điểm làm input tạo “ngắt an toàn”. Nó trả tự do cho cấu trúc ảnh chỉ khi ảnh không rỗng. sau khi trả tự do, nó sẽ đặt giá trị điểm ảnh về 0. Thu video trục tiếp Chụp một khung hình từ một webcam, hoặc từ các thiết bị quay video kĩ thuật số khác dễ dàng như load hình từ một file. Hình 2 cho chúng ta thấy một danh sách các lệnh để khởi tạo cấu trúc chụp, và lưu các video frames và đóng các giao diện chụp. Figure 2: Ví dụ chương trình chụp ảnh video và lưu chúng vào một file Giao diện chụp được khởi tạo tại dòng 19 bằng cách gọi hàm cvCaptureFromCAM(). Hàm này trả một pointer về cấu trúc cvCapture. Bạn không thể truy cập trực tiếp vào cấu trúc này. Thay vào đó bạn sẽ lưu pointer qua lệnh cvQueryFrame(). Khi bạn hoàn thành việc sử dụng thu video, bạn gọi lệnh cvReleaveCapture() để thả nổi thu video. Giống như hàm cvReleaseImage(), bạn hãy lưu địa chỉ của pointer cvCapture bằng hàm cvReleaseCapture(). Không được thả rỗi hoặc chỉnh sửa IplImage bạn nhận được từ cvQueryFrame()! Nếu bạn cần sửa dữ liệu ảnh hãy tạo một bản copy với câu lệnh như sau: // Copy the video frame IplImage *pImgToChange = cvCloneImage(pVideoFrame); // Insert your image-processing code here ... // Free the copy after using it cvReleaseImage(&pImgToChange); Chuyển đổi màu Hình 3 biểu diễn mã lệnh để chuyển đổi một ảnh màu sang một ảnh xám. OpenCV hỗ trợ chuyển đổi từ nhiều chuẩn mẫu màu khác nhau, gồm RGB, HSV, YCrCb và CIELAB. Lưu ý các hàm chuyển đổi cvCvtColor() đòi hỏi hai ảnh đầu vào. ảnh thứ nhất pRGBImag là ảnh nguồn còn ảnh thứ hai pGrayImag là ảnh sản phẩm nó sẽ chứa kết quả chuyển đổi khi hàm cvCvtColor() được gọi lại. Figure 3: Ví dụ chương trình chuyển đổi ảnh màu qua ảnh đen trắng Bởi mô hình này có nguồn ngẫu nhiên và hình ảnh kết quả để xử lý hàm và rất phổ biến trong OpenCV bạn sẽ cần chúng để tạo ảnh sản phẩm. như ở dòng 25 lệnh gọi hàm cvCreateImage() tạo ra một ảnh có cùng kích thước như ảnh ban đầu. OpenCV lưu ảnh như thế nào OpenCV lưu hình ảnh giống như C,IplImage. IPL ở trong thư viện xử lý ảnh là một thừa kế từ phiên bản trước của OpenCV. Kiểu dữ liệu IplImage được xác định trong CXCORE. Thêm vào đó là dữ liệu ảnh thô, nó chứa một số mô tả chi tiết gọi header của ảnh bao gồm: Width – chiều rộng ảnh bằng pixels Height – chiều cao ảnh bằng pixels Depth – một vài phần tử bất biến xác định nó chỉ ra số bits trên mỗi pixel trên mỗi kênh. Ví dụ nếu depth=IPL_DEPTH_8U dữ liệu cho mỗi pixel được lưu trong 8 bit với giá trị unsigned (unsigned values). nChannels – số kênh dữ liệu(từ 1 đến 4). Mỗi kênh chứa một kiểu dữ liệu pixel. Ví dụ một ảnh RGB có 3 channels – đỏ, xanh lá, xanh dương (đôi khi nó được gọi là ảnh BGR bởi vì dữ liệu pixel được lưu ở dạng xanh dương, xanh lá, đỏ). ảnh đen trắng chỉ có chứa một kênh là độ sáng của pixel. Truy xuất giá trị pixel Bạn có thể tạo nhiều kiểu hàm khác nhau nhờ OpenCV mà không cần truy xuất trực tiếp vào dữ liệu pixel thô. Ví dụ, như nhận dạng khuôn mặt, tracking, và nhận dạng chương trình được nêu sau,trong series này bạn không cần truy xuất dữ liệu thô bằng tay mà thay vào đó chúng hoạt động bằng pointer của ảnh và các cấu trúc cấp cao khác. Sự tính toán các mức pixel được thi hành bên trong các hàm của OpenCV. Tuy nhiên nếu bạn ghi thuật toán xử lya ảnh của chính bạn bạn có thể cần đến truy xuất vào dữ liệu pixel thô. Có hai cách để thực hiện như sau: Truy xuất pixel đơn giản Cách dễ dàng nhất để đọc các pixel riêng lẻ là với hàm cvGet2D(): CvScalar cvGet2D(const CvArr*, int row, int col); Hàm này lấy 3 thông số là một pointer chứa dữ liệu (CVArr*), và mảng được xếp theo hang và theo cột. Dữ liệu chứa đựng có thể có cấu trúc IplImage. Cái hang cao nhất của điểm ảnh có row = 0 và hang thấp nhất có row = chiều cao -1 Hàm cvGet2D() trả về cấu trúc dạng C là CvScalar được xác định như sau: typedef struct CvScalar { double val[4]; } CvScalar; Giá trị của mỗi điểm ảnh cho mỗi kênh được nằm trong val[i]. Với ảnh đen trắng, val[0] chứa độ sáng của điểm ảnh, ba giá trị khác được đặt bằng 0. Với ảnh có 3 kênh BGR thì blue = val[0], green = val[1],và red = val[2]. Các hàm bổ sung cvSet2D() cho phép bạn chỉnh sửa giá trị điểm ảnh. Nó được khai báo như sau: void cvSet2D(CvArr*, int row, int col, CvScalar); Truy xuất nhanh điểm ảnh Mặc dù hàm cvGet2D() và cvSet2D() rất dễ dàng sử dụng nếu bạn muốn truy xuất nhiều hơn vài giá trị điểm ảnh và gặp vấn đề bạn sẽ phải đọc dữ liệu trực tiếp từ bộ nhớ đệm IplImage.imageData. Dữ liệu ảnh trong bộ nhớ đệm được lưu dưới dạng 1 chiều, trong chủ yếu một hang. Điều đó có nghĩa tất cả các giá trị điểm ảnh trong hang đầu tiên được liệt kê ra trước tiếp theo sau là giá trị điểm ảnh của hang thứ hai và cứ tiếp tục như thế cho đến hang cuối cùng. Bởi vì dữ liệu điểm ảnh được xếp hàng và chèn thêm vào nếu cần thiết vì thế mỗi dòng đều bắt đầu là bội của 4 byte. Ngoài ra IplImage.WidthStep biểu thị số byte dữ liệu điểm ảnh của mỗi dòng. Đó là dòng thứ i bắt đầu ở IplImage.imageData+i*IplImage.widthStep. IplImage.imageData định nghĩa theo kiểu char* vì vậy bạn cần hiểu kiểu dữ liệu này, ví dụ, nếu dữ liệu ảnh của bạn ở dạng unsigned bytes ( một kiểu dữ liệu phổ biến), bạn nên hiểu mỗi giá trị theo kiểu unsigned char* trước khi gán, hoặc sử dụng theo cách khác. Nếu bạn truy xuất dữ liệu từ một ảnh đen trắng ( kênh đơn), và độ sâu của dữ liệu là 8 bits( một byte trong mỗi pixel), bạn nên truy xuất hàng cột của điểm ảnh như sau: pixel[row][col] = ((uchar*) (pImg->imageData + row*pImg->widthStep + col)); trong ảnh đa kênh, kênh dữ liệu quét chẵn lẻ xen kẽ nhau. Đây là một đoạn mã lệnh để truy xuất các giá trị điểm ảnh blue, green, red: step = pImg->widthStep; nChan = pImg->nChannels; // = 3 for a BGR image buf = pImg->imageData; blue[row][col] = ((uchar*)(buf + row*widthStep + nChan*col); green[row][col] = ((uchar*)(buf + row*widthStep + nChan*col + 1); red[row][col] = ((uchar*)(buf + row*widthStep + nChan*col + 2); cuối cùng nếu độ sâu ảnh của bạn lớn hơn 8 bits ví dụ IPL_DEPTH_32S. bạn cần chuyển đổi byte bội số cho mỗi giá trị và nhân nó với bộ đệm bằng số byte dữ liệu với độ sâu của ảnh. Điều đó rất khó thành công tuy nhiên bạn sẽ gặp phải hoàn cảnh đó và bạn sẽ phải truy xuất trực tiếp giá trị các điểm ảnh Phần 7 TRUYỀN DỮ LIỆU BẰNG MODULE RF Đặc điểm của module HM TR: Công nghệ FSK, chế độ bán song công, nhưng nhiễu rất mạng Băng tần ISM, không cần đăng ký tần số Có thể cấu hình tần số và có thể sử dụng ứng dụng FDMA Có thể chọn được độ lệch tần số phát sóng và băng thông thu Giao thức dịch tự điều khiển và rất dễ sử dụng Tốc độ truyền dữ liệu có thể lựa chọn nhiều phạm vi Có chân Enable để điều khiển công suất cho những ứng dụng khác nhau Độ nhạy cao, phạm vi truyền tải rộng Giao tiếp UART tiêu chuẩn, TTL hoặc RS232 với mức logic tùy chọn Rất gọn nhẹ độ tin cậy cao, kích thước nhỏ gọn, dễ dàng lắp đặt Không có chỉnh sóng trên thiết bị Ứng dụng: Điều khiển từ xa, điều khiển thiết bị đo Đo không dây Điều khiển truy xuất Nhận dạng tách lọc Thu thập dữ liệu Thiết bị điện máy trong nhà Sản phẩm cho căn nhà thông minh Quản lý trẻ em Hình dạng thực tế: Sơ đồ chân Chân Tên chân Mô tả VCC Nguồn +5V DTX Phát dữ liệu Module phát DRX Nhận dữ liệu Module nhận CONFIG Chọn chế độ Nếu chân này ở mức cao nó sẽ đặt thông số làm việc ENABLE Tính năng làm việc Nếu chân này ở mức cao thì mạch bắt đầu phát tín hiệu Module ứng dụng Module có hai chế độ hoạt động: chế độ giao tiếp và chế độ cấu hình được xác định bởi trạng thái của chân config khi nguồn mở Config = low: hoạt động ở chế độ thu phát Config = high: hoạt động ở chế độ cấu hình Chế độ cấu hình Nếu chân config ở mức thấp khi nguồn mở thì module sẽ hoạt động ở chế độ truyền nhận thông tin. Module cung cấp kết nối RS232 để kết nối với máy tính hoặc TTL với MCU trực tiếp Nó có thể hoạt động với cấu hình mặc định như sau (9.6kbps, 8bits, không kiểm tra 1 bit dừng) Khi dữ liệu truyền tải dưới 9.6kbps thì module HM TR hỗ trợ tiếp tục truyền tải và tốc độ tối đa có thể đạt 1mb tuy nhiên tốc độ truyền tải không vượt quá 32bytes ở tốc độ cao. HM-TR hoạt động ở chế độ bán song công khi nhận 32bytes dữ liệu ở cổng nối tiếp nó sẽ phát dữ liệu ra một lần. nếu dữ liệu nhận được ít hơn 32bytes thì module sẽ đợi 30ms và gửi lại. dữ liệu được gửi tức thì và việc gửi 32bytes là cần thiết. Sau khi phát dữ liệu thì module sẽ chuyển sang chế độ nhận dữ liệu một cách tự động và mất 5ms. Chân enable dùng để điều khiển lượng điện tiêu thụ nếu chân này ở mức thấp thì module hoạt động ở chế độ ngủ đông. Người dùng có thể dùng chân này để điều khiển chu kỳ nhận năng lượng. Chế độ configuration(cấu hình) Nếu chân config ở mức cao thì module sẽ hoạt động ở chế độ cấu hình ở trong chế độ này module truyền tin với host bằng định dạng nối tiếp cố định Phần mềm cài đặt HM-TR Bạn có thể kiểm tra thông số của HM-TR và cài đặt các thông số bằng tiện ích HM-TR setup utility Nút read: đọc các thông số mà module đang dùng Nút write:ghi cấu hình mới vào module Nút default: chế độ mặc định Phần 8: CHƯƠNG TRÌNH ĐIỀU KHIỂN I.Bảng điều khiển: Bảng điều khiển Robot. II.LƯU ĐỒ GIẢI THUẬT: 1.Giải thuật tổng thể: (1) - Thông tin về vị trí đối tượng (2) -Ccamera (3) - Máy tính thu thập các khung ảnh bằng thư viện OPENCV (4) - Quá trình xử lý các khung ảnh thu được thông tin về vị trí của đối tượng (5) - Dựa vào các thông tin để tính toán lệnh điều khiển (6) - Gửi thông tin điều khiển qua cổng COM (7) - Mạch điều khiển các động cơ (8) – Motor - điều chỉnh tốc độ quay của động cơ (thay đổi vị trí của robot) Quá trình thu và xử lý ảnh được tóm tắt qua lưu đồ bên: 2.Giải thuật xử lý ảnh: 3Gải thuật robot: III. Chương trình điều khiển: Giải thuật điều khiển: 1.Phần C#: using System; using System.Collections.Generic; using System.ComponentModel; using System.Data; using System.Drawing; using System.Text; using System.Windows.Forms; using System.Threading; using OpenCVDotNet; using Media; using System.IO; using System.IO.Ports; namespace nhandang { public partial class frm_nhandang : Form { #region Khaibao //========================================== private CVImage range_image = null; private CVImage image = null; private CVImage imageSave = null; private CVCapture Camera = new CVCapture(); private CVHistogram initialHistogram = null; private CVHistogram laserHis = null; private DialogResult thoat = new DialogResult(); //========================================== private int Freq_update = 0; //========= Port control =================== private int control_R = 0; private int control_L = 0; private int Hex_PositionX; private int Hex_PositionY; #endregion //---------------------------------------------------------- public frm_nhandang() { InitializeComponent(); } //-------------------------------------------------------- private void frm_nhandang_Load(object sender, EventArgs e) { ReadData(); Binsize_histo.Value = 20;//giá trị ban đầu trackbar khi load form iteration_trackBar.Value = 1; lb_bieudomau.Text = Binsize_histo.Value.ToString();//hiển thị giá trị lên label lb_phantichanh.Text = iteration_trackBar.Value.ToString(); } //=================================================================== #region Xulyanh private void Xulyanh() { if (image == null) return; image.Resize(Nhandang_pic.Width, Nhandang_pic.Height); // calculate the back projection of the current image on top of the initial histogram. using (CVImage Picture_255 = image.CalcBackProject(this.initialHistogram)) { Region_select.Image = Picture_255.ToBitmap(); CVConnectedComp Move = Picture_255.MeanShift(Nhandang_pic.SelectionRect, (int)Convert.ToInt32(lb_phantichanh.Text), 2000, 5000); Nhandang_pic.SelectionRect = Move.Rect; } Freq_update = (Freq_update + 1) % 500;//500 x 20ms lan dem cap nhat mot lan //Counter.Text = Convert.ToString(Freq_update); if (Freq_update == 0) UpdateHistogram(); RefreshImages(); } //--------------------------------------------------------------- private void UpdateHistogram() //Cập nhật hình ảnh cũ (nếu có) hoặc mới, tính toán vùng hiển thị { if (initialHistogram != null) initialHistogram.Release(); //nếu ko có ảnh thì return if (image == null) return; //+++++++++++++++++++++++++++++++++ int[] bpBins = { Binsize_histo.Value, Binsize_histo.Value, Binsize_histo.Value }; CVPair[] bpRanges = { new CVPair(0, 255), new CVPair(0, 255), new CVPair(0, 255) }; //tính toán biểu đồ màu vùng nhận dạng để hiển thị image.RegionOfInterest = Nhandang_pic.SelectionRect; this.initialHistogram = image.CalcHistogram(bpBins, bpRanges); image.ResetROI(); RefreshImages(); } //=================================================================== private void RefreshImages() //Làm tươi ảnh { //------------------------------------------------ image.RegionOfInterest = Nhandang_pic.SelectionRect; image.ResetROI(); image.Resize(Nhandang_pic.Width, Nhandang_pic.Height); Nhandang_pic.Image = image.ToBitmap(); } //---------------------------------------------------------------- private void timer1_Tick(object sender, EventArgs e)//timer cho phép hiển thị video { image.Dispose();//reset image image = null; image = Camera.QueryFrame(); try { if (image == null) { Camera.Dispose(); Camera.Restart(); } if ((initialHistogram != null) && (flashPic == false)) Xulyanh(); else { flashPic = false; image.Resize(Nhandang_pic.Width, Nhandang_pic.Height); //định dạng ảnh nhận dạng Nhandang_pic.Image = image.ToBitmap();//show camera } //hiển thị vùng video nhận dạng image.RegionOfInterest = System.Drawing.Rectangle.Inflate(Rectangle.FromLTRB ( Nhandang_pic.SelectionRect.X, Nhandang_pic.SelectionRect.Y, Nhandang_pic.SelectionRect.X + Nhandang_pic.SelectionRect.Width + 8, Nhandang_pic.SelectionRect.Y + ((Nhandang_pic.SelectionRect.Width + 8) * 100) / 80 ), 0, 0); Region_select.Image = image.ToBitmap(); //cho phép hiển thị vùng nhận dạng //Đưa dữ liệu từ hình ảnh ban đầu lên biểu đồ màu initHisto.ShowHistogram(image); //xác định tọa độ X Y và hiện lên textbox PositionX.Text = Convert.ToString(Nhandang_pic.SelectionRect.X + (Nhandang_pic.SelectionRect.Width / 2));//chia 2 để lấy tâm PositionY.Text = Convert.ToString(Nhandang_pic.SelectionRect.Y + (Nhandang_pic.SelectionRect.Height / 2)); Hex_PositionX = (int)((float)(Nhandang_pic.SelectionRect.X + (Nhandang_pic.SelectionRect.Width / 2))); // initPicture.Width * 255); } catch { timer1.Enabled = false; MessageBox.Show("Có lỗi từ Camera, vui lòng kiểm lại Camera kết nối", "Thông báo", MessageBoxButtons.OK, MessageBoxIcon.Warning); } } //------------------------------------------------------------- private void TimerWaitforResuft_Tick(object sender, EventArgs e) { lbFilename.Text = "Đã nhận dạng OK!"; } #endregion //==================================================================== //các nút điều khiển #region NUT private void start_camera_Click(object sender, EventArgs e) { tab_all.SelectTab(0); image.Dispose(); image = Camera.QueryFrame(); if (image != null) { if (timer1.Enabled == true) { timer1.Enabled = false; open_camera.Text = "Open Camera"; start_camera.Text = "Open Camera"; } else { timer1.Enabled = true; open_camera.Text = "Close Camera"; start_camera.Text = "Close Camera"; } } else MessageBox.Show("Không kết nối được với Camera", "Thông báo"); } //----------------------------------------------------------------- private void start_image_Click(object sender, EventArgs e) { OpenFileDialog openFile = new OpenFileDialog(); openFile.Title = "Open Image";//tên cửa sổ openFile.Filter = "File ảnh(*.jpg,*.bmp)|*.jpg;*.bmp";//loại ảnh if (openFile.ShowDialog() != DialogResult.OK) return; //show image image = new CVImage(openFile.FileName); Nhandang_pic.Image = image.ToBitmap(); } private void open_camera_Click(object sender, EventArgs e) { tab_all.SelectTab(0); image = Camera.QueryFrame(); if (image != null) { if (timer1.Enabled == true) { timer1.Enabled = false; open_camera.Text = "Open Camera"; start_camera.Text = "Open Camera"; } else { timer1.Enabled = true; open_camera.Text = "Close Camera"; start_camera.Text = "Close Camera"; } } else MessageBox.Show("Không kết nối được với Camera ", "Thông báo"); } //--------------------------------------------------------------- private void open_image_Click(object sender, EventArgs e) { OpenFileDialog openFile = new OpenFileDialog(); openFile.Title = "Open Image"; openFile.Filter = "File ảnh(*.jpg,*.bmp)|*.jpg;*.bmp"; if (openFile.ShowDialog() != DialogResult.OK) return; image = new CVImage(openFile.FileName); Nhandang_pic.Image = image.ToBitmap();// bitmap hien thi anh } //================================================================ private void bt_exit_Click(object sender, EventArgs e) { thoat = MessageBox.Show("Bạn muốn thoát khỏi chương trình?", "Thông báo", MessageBoxButtons.YesNo, MessageBoxIcon.Question); if (thoat == DialogResult.Yes) { Camera_center(); Camera_exit(); this.Dispose(); } } #endregion //==================================================================== #region nhandang private void WriteRecognize(string Value) { StreamWriter SW = new StreamWriter("D:\\nhap\\Recognize.crt"); SW.WriteLine(Value); SW.Close(); } //-------------------------------------------------------------- private void WriteMatlapRun(string value) { StreamWriter SW = new StreamWriter("C:\\in\\Matlap.crt"); SW.WriteLine(value); SW.Close(); } #endregion //----------------------------------------------------------- private void initPicture_MouseClick(object sender, MouseEventArgs e) { if (e.Button == MouseButtons.Right) initialHistogram = null; } //------------------------------------------------------------- private void initPicture_MouseDown(object sender, MouseEventArgs e) { if (e.Button == MouseButtons.Left) initialHistogram = null; } //------------------------------------------------------------- private void initPicture_MouseUp(object sender, MouseEventArgs e) { try { if (e.Button == MouseButtons.Left) UpdateHistogram(); TimerWaitforResuft.Enabled = true;//timer cho lbfilename lbFilename.Text = "Đang nhận dạng.."; } catch { MessageBox.Show("Có lỗi!Không thể nhận dạng.", Application.ProductName, MessageBoxButtons.OK, MessageBoxIcon.Information); } } //---------------------------------------------------------------- //Thanh trượt phân tích biểu đồ màu Track_bar #region Track_bar private void Binsize_histo_ValueChanged(object sender, EventArgs e) { lb_bieudomau.Text = Binsize_histo.Value.ToString(); initHisto.BinsPerChannel = Binsize_histo.Value; UpdateHistogram();//cập nhật histogtam } private void Binsize_histo_Scroll(object sender, EventArgs e) { lb_bieudomau.Text = Binsize_histo.Value.ToString(); UpdateHistogram();//cập nhật histogram } private void iteration_trackBar_Scroll(object sender, EventArgs e) { lb_phantichanh.Text = iteration_trackBar.Value.ToString(); } #endregion #region Port private void optionsToolStripMenuItem_Click_1(object sender, EventArgs e) { tab_all.SelectTab(1); } //-------------------------------------------------------------- private void select_port_DropDown(object sender, EventArgs e) { GetPort(); lbComport.Visible = true; } //--------------------------------------------------------------- private void select_port_SelectedIndexChanged(object sender, EventArgs e) { PortSelectOpen(); } //----------------------------------------------------------------- private void GetPort() { string[] ports = SerialPort.GetPortNames(); select_port.Items.Clear(); // Add all port names to the combo box: foreach (string port in ports) { select_port.Items.Add(port); } } //------------------------------------------------------ private void PortSelectOpen() { try { if (serialPort1.IsOpen) serialPort1.Close(); serialPort1.PortName = select_port.SelectedItem.ToString(); lbport.Text = serialPort1.PortName + ": 9600,8N1"; //Mo Port duoc chon neu co the } catch { } try { serialPort1.Open(); lbComport.Text = serialPort1.PortName; WriteData(); } catch { //Hien thi message neu khong the mo MessageBox.Show("Cổng nối tiếp " + serialPort1.PortName + " không thể mở được!", "Serial Port", MessageBoxButtons.OK, MessageBoxIcon.Warning); select_port.SelectedText = ""; lbport.Text = "Chọn cổng nối tiếp!"; lbComport.Text = ""; } } //--------------------------------------------------------------------------- private void lbComport_TextChanged(object sender, EventArgs e) { serialPort1.PortName = lbComport.Text; serialPort1.Open(); } #endregion //===================================================================================== #region DevidePort private void WriteData() { StreamWriter SW = new StreamWriter("Data.ini"); SW.WriteLine(""); SW.WriteLine(serialPort1.PortName); SW.WriteLine("End section>"); SW.Close(); } private void ReadData() { StreamReader SR = new StreamReader("Data.ini"); if (serialPort1.IsOpen) serialPort1.Close(); SR.ReadLine();//read section serialPort1.PortName = SR.ReadLine(); SR.Close(); lbport.Text = serialPort1.PortName + ":9600,8N1"; try { serialPort1.Open(); lbComport.Text = serialPort1.PortName; } catch { //Hien thi message neu khong the mo MessageBox.Show("Cổng nối tiếp " + serialPort1.PortName + " không thể mở được! Vui lòng chọn cổng khác trước khi sử dụng", "Serial Port", MessageBoxButtons.OK, MessageBoxIcon.Warning); tab_all.SelectTab(1); select_port.SelectedText = ""; lbport.Text = "Chọn cổng nối tiếp!"; lbComport.Text = ""; } } #endregion //======================================================================= #region Robot control private void bt_nhandang_Click(object sender, EventArgs e) { if (box_tdong.Checked & TimerWaitforResuft.Enabled==true) { //box_tdong.Checked = false; bt_startnhandang.Text = "Stop"; timer3.Enabled = true; timer4.Enabled = true; } else { bt_startnhandang.Text = "Start"; timer3.Enabled = false; timer4.Enabled = false; } if (timer1.Enabled == false) { bt_startnhandang.Text = "Start"; timer3.Enabled = false; timer4.Enabled = false; MessageBox.Show("Bạn chưa mở Camera!", "Thông báo", MessageBoxButtons.OK, MessageBoxIcon.Warning); } //----------------------------------------------------------------- //Đã truyền dc dữ liệu ra cổng com dk_td; //xác định hướng quay cho camera private void box_tdong_CheckStateChanged(object sender, EventArgs e) { if (box_tdong.Checked) { Dung(); box_btay.Enabled = false; } else { box_btay.Enabled = true; } } //----------------------------------------------------------------------5 private void box_btay_CheckStateChanged(object sender, EventArgs e) { if (box_btay.Checked) { ca_L.Enabled = true; ca_R.Enabled = true; bt_tien.Enabled = true; bt_lui.Enabled = true; bt_phai.Enabled = true; bt_trai.Enabled = true; bt_tdo1.Enabled = true; bt_tdo2.Enabled = true; bt_tdo3.Enabled = true; } else { ca_L.Enabled = false ; ca_R.Enabled = false; bt_tien.Enabled = false; bt_lui.Enabled = false; bt_phai.Enabled = false; bt_trai.Enabled = false; bt_tdo1.Enabled = false; bt_tdo2.Enabled = false; bt_tdo3.Enabled = false; } } //----------------------------------------------------------------------- private void timer3_Tick(object sender, EventArgs e)//định hướng quay cho camera { try { if (control_L == 1) { Camera_RotaL(); } if (control_R == 1) { Camera_RotaR(); } if (control_L == 2) { C_outL(); } if (control_R == 2) { C_outR(); } if (control_R == 0 & control_L==0) { Camera_center(); //Camera_exit(); } if (box_tdong.Checked == false) { Camera_center(); Camera_exit(); timer3.Enabled = false; timer4.Enabled = false; } } catch { timer3.Enabled = false; timer4.Enabled = false; box_tdong.Checked = false; bt_startnhandang.Text = "Start"; MessageBox.Show("Vui lòng kiểm tra lại kết nối!", "Comport", MessageBoxButtons.OK, MessageBoxIcon.Warning);// lỗi khi chưa kết nối cổng com khi check dk_td } } //----------------------------------------------------------- private void timer4_Tick(object sender, EventArgs e) //tính toán giá tri X de xác định camera quay trái hay phải { if (box_tdong.Checked) { //Quay trai if (Hex_PositionX > 180 & Hex_PositionX <270) { control_L = 1; } if (Hex_PositionX < 200) { control_L = 0; } if (Hex_PositionX > 270) { control_L = 2; } //--------------------------------------------- //Quay phai if (Hex_PositionX >45 & Hex_PositionX < 100) { control_R = 1; } if (Hex_PositionX > 100 ) { control_R = 0; } if (Hex_PositionX < 45) { control_R = 2; } } } //---------gửi dữ liệu ra cổng com------------------ private void Camera_RotaL() { int RotaLeft = 239; serialPort1.Write(new byte[] { (byte)RotaLeft }, 0, 1); } //------------------------------------------------------------ private void Camera_RotaR() { int RotalRight = 247; serialPort1.Write(new byte[] { (byte)RotalRight }, 0, 1); } //------------------------------------------------------------- private void C_outR() { int out_r = 251; serialPort1.Write(new byte[] { (byte)out_r }, 0, 1); } private void C_outL() { int out_l = 223; serialPort1.Write(new byte[] { (byte)out_l }, 0, 1); } private void Camera_center() { int center = 231; serialPort1.Write(new byte[] { (byte)center }, 0, 1); } private void Camera_exit() { int Car_exit = 255; serialPort1.Write(new byte[] { (byte)Car_exit }, 0, 1); } #endregion //==================================================================== #region DK_Bangtay #region Nut private void bt_tien_Click(object sender, EventArgs e) { R_tien(); } private void bt_lui_Click(object sender, EventArgs e) { R_lui(); } private void bt_trai_Click(object sender, EventArgs e) { R_lelf(); } private void bt_phai_Click(object sender, EventArgs e) { R_right(); } private void ca_L_Click(object sender, EventArgs e) { Camera_trai(); } private void ca_R_Click(object sender, EventArgs e) { Camera_phai(); } private void bt_dung_Click(object sender, EventArgs e) { Camera_exit(); Dung(); } #endregion //============================================================= #region data private void R_tien() { int R_t = 191; serialPort1.Write(new byte[] { (byte)R_t }, 0, 1); } //------------------------------------------------------------ private void R_lui() { int R_l = 63; serialPort1.Write(new byte[] { (byte)R_l }, 0, 1); } //------------------------------------------------------------ private void R_right() { int R_p = 127; serialPort1.Write(new byte[] { (byte)R_p }, 0, 1); } //------------------------------------------------------------- private void R_lelf() { int R_t = 253; serialPort1.Write(new byte[] { (byte)R_t }, 0, 1); } //----------------------------------------------------------- private void Camera_phai() { int C_p = 252; serialPort1.Write(new byte[] { (byte)C_p }, 0, 1); } private void Camera_trai() { int C_t = 254; serialPort1.Write(new byte[] { (byte)C_t }, 0, 1); } private void Dung() { int stop = 255; serialPort1.Write(new byte[] { (byte)stop }, 0, 1); } private void Start() { int start = 240; serialPort1.Write(new byte[] { (byte)start }, 0, 1); } private void toc_do_1() { int td1 = 31; serialPort1.Write(new byte[] { (byte)td1 }, 0, 1); } private void toc_do_2() { int td2 = 15; serialPort1.Write(new byte[] { (byte)td2 }, 0, 1); } private void toc_do_3() { int td3 = 7; serialPort1.Write(new byte[] { (byte)td3 }, 0, 1); } #endregion #endregion } } 2.Chương trình vi xử lý: $MOD51 ;CAC QUY UOC BAN DAU ;================PORT 2====================================== PRO BIT P2.0 ;chan nay nhan tin hieu tu IC to hop chon chuong trinh SELECT1 BIT P2.1 ;bit to hop chon chuong trinh 1 SELECT2 BIT P2.2 ;bit to hop chon chuong trinh 2 SELECT3 BIT P2.3 ;bit to hop chon chuong trinh 3 SELECT4 BIT P2.4 ;bit to hop chon chuong trinh 4 MAU BIT P2.5 ;bang 0 mau Xanh,1 mau do LED BIT P2.6 ;bao khi di qua 1 giao diem hoac tuy y lua chon WAITING BIT P2.7 ;bao trang thai sang sang (ready) ;================PORT 3====================================== ENCODER1 BIT P3.4 ;chan dem xung tu encoder 1 ENCODER2 BIT P3.5 ;chan dem xung tu encoder 2 RL4 BIT P3.6 ;dao chieu motor 4 BANH TRAI RL5 BIT P3.7 ;dao chieu motor 5 BANH PHAI RL1 BIT P1.0 ;dao chieu motor 1 RL2 BIT P1.1 ;dao chieu motor 2 RL3 BIT P1.2 ;dao chieu motor 3 DK CAMERA SELECT_MODE_A BIT P3.0 ;LSB NUT BEN TAY TRAI SELECT_MODE_B BIT P3.1 ;MSB NUT BEN TAY PHAI MAT_THAN_TO BIT P3.2 ;CAM BIEN BAO DU KHOANG CACH MAT_THAN_NHO BIT P3.3 ;CAM BIEN LUI ;==============SENSOR======================================== LEFT3 BIT P0.0 ;cam bien trai thu 3 LEFT2 BIT P0.1 ;cam bien trai thu 2 LEFT1 BIT P0.2 ;cam bien trai thu 1 CENTERL BIT P0.3 ;cam bien giua trai CENTERR BIT P0.4 ;cam bien giua phai RIGHT1 BIT P0.5 ;cam bien phai thu 1 RIGHT2 BIT P0.6 ;cam bien phai thu 2 RIGHT3 BIT P0.7 ;cam bien phai thu 3 ;==============MOTOR========================================= MOTORL DATA CCAP0H ; P1.3 ;dieu xung cho motor trai MOTORR DATA CCAP1H ; P1.4 ;dieu xung cho motor phai MOTOR1 DATA CCAP2H ; P1.5 ;dieu xung cho motor 3 MOTOR2 DATA CCAP3H ; P1.6 ;dieu xung cho motor 4 MOTOR3 DATA CCAP4H ; P1.7 ;dieu xung cho motor 5 ;=====================VAN TOC CHO BANH TRAI============================= V_MOTORL_VT1 DATA 40H V_MOTORL_VT2 DATA 41H V_MOTORL_VT3 DATA 42H V_MOTORL_VT4 DATA 43H V_MOTORL_VT5 DATA 44H V_MOTORL_VT6 DATA 45H V_MOTORL_VT7 DATA 46H V_MOTORL_VT8 DATA 47H ;===============CAC BIT TRUNG GIAN============================= BLUE BIT 00H ;bit bao dang la doi xanh hay do(CLR - Xanh,SETB-Do) DEM_PHU BIT 07H ;LAY CANH XUONG CUA SENSOR DE NHAN DIEN VACH NHO_TRAI BIT 03H ;da re trai truoc do NHO_PHAI BIT 04H ;da re phai truoc do BIT_RUN BIT 0BH ;CHO PHEP CHAY TRACKING(SETB) ;============================================================================== L_BYTE DATA 30H ;byte thap dung trong counter H_BYTE DATA 31H ;byte cao dung trong counter LUU_VACH DATA 32H ;luu so vach trung gian truoc khi xu ly van de(T0ISR) VUNG DATA 37H ;LUU S VUNG (DO PHAN VUNG CAM BIEN)(KHOANG 16 VUNG KHAC NHAU) VACH DATA 07H ;luu so vach da di qua = so giao diem(R7) ;===========BAT DAU VIET CHUONG TRINH================== ;Cac thanh ghi da su dung R0,R1,R7,R2,R6,R3,R4,R5 ;R0 DUNG DE LAY MAU TIN HIEU(CHONG NHIEU) ;R1 DUNG TRONG HAM DELAY(CO THE LAY LAM TRUNG GIAN) ;R2 LUU SO VACH NGANG(TRUNG GIAN CHO VIEC TANG HAY GIAM VACH (R7)) ;R3 LUU SO VACH TRUNG GIAN TRUOC KHI XU LY (CTRINH CON DI? VACH) ;R7 LUU SO VACH DA DI QUA ;A TRUNG GIAN CHO VIEC KTRA LOI KHI QUA VACH NGANG MA CAM BIEN KO LEN(DUNG TRONG T0ISR) ;B LUU CAM BIEN KHI XAC DINH VACH(DUNG TRONG XAC DINH DEM VACH(TRACKING)) ;R5 AP DUNG TRONG CHUONG TRINH CON DK PWM ORG 0000H ;reset he thong SJMP MAIN ORG 0003H ;ngat ngoai 0 LJMP INT0ISR ORG 000BH ;ngat timer 0 LJMP T0ISR ORG 0013H ;ngat ngoai 1 LJMP INT1ISR ORG 001BH ;ngat timer 1 LJMP T1ISR ORG 002BH LJMP T2ISR ORG 0030H ;ngat port noi tiep SJMP MAIN ORG 004BH LJMP POWER_DOWN MAIN: MOV SCON,#12H ;CHO PHEP PORT NOI TIEP HOAT DONG ;====== ====>>==== ;ngung hoat dong tat ca cac timer CLR TF0 CLR TR1 CLR TF1 MOV TH0,#00H MOV TL0,#00H MOV TH1,#00H MOV TL1,#00H SETB EA ;ngat toan cuc ;-- ----------------------------------------------- CLR NHO_TRAI CLR NHO_PHAI MOV LUU_VACH,#00H ; CLR BIT_RUN ;BO QUA QUET CAM BIEN ;---------- -------------------------------------------- MOV MOTOR1,#00H MOV MOTOR2,#00H MOV MOTOR3,#00H LCALL TEST_MAU JNB MAU,MODE_BLUE MODE_RED: CLR WAITING LCALL DOI1S SETB WAITING LCALL DOI1S CLR WAITING LCALL DOI1S SETB WAITING LCALL DOI1S LCALL RESET_ROBOT LCALL TEST_MAU ACALL SCAN_PRO_RED LJMP MAIN MODE_BLUE: CLR WAITING LCALL DOI1S SETB WAITING LCALL DOI1S LCALL RESET_ROBOT LCALL TEST_MAU ACALL SCAN_PRO_BLUE LJMP MAIN ;======================TEST_MAU===================================== TEST_MAU: MOV R0,#255 T_MAUXANH: JNB MAU,GIAM_XANH SJMP T_MAUDO GIAM_XANH: DJNZ R0,T_MAUXANH CLR BLUE ;DOI XANH SETB LED ;DEN LED SANG SJMP EXIT_TEST_MAU T_MAUDO: MOV R0,#255 CHO_MAU: JB MAU,GIAM_DO SJMP T_MAUXANH GIAM_DO: DJNZ R0,CHO_MAU SETB BLUE ;DOI DO CLR LED ;DEN LED SANG EXIT_TEST_MAU: RET ;========================SCAN_PRO_RED===================================== SCAN_PRO_RED: JNB MAU,MODE_BLUE LCALL DOI50MS LCALL CHAYR1 LJMP SCAN_PRO_RED CHAYR1: CLR WAITING LCALL DOI1S LCALL PRO_R1 LJMP EXIT_SCAN_PRO_R EXIT_SCAN_PRO_R: RET ;========================SCAN_PRO_BLUE===================================== SCAN_PRO_BLUE: JB MAU,MODE_RED LCALL DOI50MS LCALL CHAYB1 LJMP SCAN_PRO_BLUE CHAYB1: CLR WAITING LCALL DOI1S LCALL PRO_B2 LJMP EXIT_SCAN_PRO_B EXIT_SCAN_PRO_B: RET ;=======BAT DAU CHAY CHUONG TRINH=========================== START: ============>>============================================ PRO_R1: LCALL TOC_DO_CHAM LCALL SCAN_CHUONG_TRINH SJMP PRO_R1 RET ;==============================================<<<===PRO_R2 ;============-CAC CONG TAC HANH TRINH============= SW1: ;CONG TAC PHAI SETB SELECT1 ;1 CLR SELECT2 ;0 SETB SELECT3 ;1 SETB SELECT4 ;1 LCALL DOI50ms RET ;================================================ SW2: ;CONG TAC TRAI CLR SELECT1 ;0 SETB SELECT2 ;1 SETB SELECT3 ;1 SETB SELECT4 ;1 LCALL DOI50ms RET ;================================================= SW3: ;CONG TAC TRAI SETB SELECT1 ;1 SETB SELECT2 ;1 SETB SELECT3 ;1 SETB SELECT4 ;1 LCALL DOI50ms RET ;================================================= STOP_CAMERA: MOV MOTOR1,#5 CLR RL3 LCALL DOI1S SETB RL3 CLR TR1 MOV MOTOR1,#00H RET STOP: MOV MOTOR2,#10 MOV MOTOR3,#10 CLR RL4 CLR RL5 LCALL DOI1S SETB RL4 ;TRA ROLE VE TRANG THAI BINH THUONG SETB RL5 CLR TR1 ;NGUNG NGAT T1,CHO ROBOT STOP LAI MOV MOTOR2,#00H MOV MOTOR3,#00H RET ;======================================================= CHAY_TOI: SETB RL4 SETB RL5 LCALL DOI50MS MOV MOTOR2,#180 ;BANH TRAI MOV MOTOR3,#200 ;BANH PHAI RET CHAY_LUI: CLR RL4 CLR RL5 LCALL DOI50MS MOV MOTOR2,#100 MOV MOTOR3,#170 RET CUA_PHAI: MOV MOTOR2,#40 MOV MOTOR3,#40 CLR RL5 SETB RL4 RET CUA_TRAI: MOV MOTOR2,#40 MOV MOTOR3,#40 CLR RL4 SETB RL5 RET QUAY_TRAI_C: SETB RL3 MOV MOTOR1,#20 RET QUAY_PHAI_C: CLR RL3 MOV MOTOR1,#20 RET RESET_ROBOT: LCALL STOP LCALL DOI1S LCALL STOP_CAMERA RET ;-------------xxxxxxx------------xxxxxxx------------------ ENABLE_INT0: CLR IT0 ;cho phep ngat ngoai 0 CANH AM SETB EX0 ;BAT DAU CHAY NGAT NGOAI 0 SETB EA ;ngat toan cuc dc set len 1 RETI ;======= ====INT0ISR================================== INT0ISR: RETI ;====== =====INT1ISR================================== INT1ISR: RETI ;=== ===T0ISR==================================== T0ISR: CLR TR0 CLR TF0 RETI ;=== ==T1ISR==================================== T1ISR: CLR TR1 CLR TF1 RETI T2ISR: RETI ;======== =====CHE DO NHIEU NGUON====================== POWER_DOWN: MOV MOTORL,#00H MOV MOTORR,#00H LCALL DELAY800MS LCALL DELAY800MS MOV IEN1,#00000000B RETI SCAN_CHUONG_TRINH: SETB WAITING VUNG_TRUNG_TAM: ;01234567 MOV C,CENTERL ;11100111 ORL C,CENTERR JC VUNG_PHAI_1 MOV VUNG,#00H LJMP EXIT_TRACKING VUNG_PHAI_1: MOV B,P0 ;01234567 CPL B.3 ;LAY BU CPL B.5 MOV C,CENTERR ;11110111 ORL C,B.3 ORL C,B.5 JC VUNG_LAC_PHAI MOV VUNG,#01H SETB NHO_PHAI CLR NHO_TRAI LJMP EXIT_TRACKING VUNG_LAC_PHAI: MOV B,P0 ;01234567 ;11111011 CPL B.4 CPL B.6 MOV C,RIGHT1 ORL C,B.4 ORL C,B.6 JC VUNG_TRAI_1 MOV VUNG,#02H SETB NHO_PHAI CLR NHO_TRAI LJMP EXIT_TRACKING VUNG_TRAI_1: MOV B,P0 ;01234567 CPL B.2 ;LAY BU CPL B.4 MOV C,CENTERL ;11101111 ORL C,B.2 ORL C,B.4 JC VUNG_LAC_TRAI MOV VUNG,#03 SETB NHO_TRAI CLR NHO_PHAI LJMP EXIT_TRACKING VUNG_LAC_TRAI: MOV B,P0 ;01234567 ;11011111 CPL B.1 CPL B.3 MOV C,LEFT1 ORL C,B.1 ORL C,B.3 JC KIEM_TRA_R_TIEN MOV VUNG,#04 SETB NHO_TRAI CLR NHO_PHAI LJMP EXIT_TRACKING ;------------------------------------------------------------------- KIEM_TRA_R_TIEN: MOV B,P0 ;01234567 ;11111101 CPL B.5 CPL B.7 MOV C,RIGHT2 ORL C,B.5 ORL C,B.7 JC KIEM_TRA_R_LUI MOV VUNG,#11 LJMP EXIT_SCAN_PHIM KIEM_TRA_R_LUI: MOV C,RIGHT2 ;01234567 ORL C,RIGHT3 ;11111100 JC KIEM_TRA_R_PHAI MOV VUNG,#12 LJMP EXIT_SCAN_PHIM KIEM_TRA_R_PHAI: MOV C,RIGHT3 ;01234567 MOV B,P0 ;11111110 CPL B.6 ORL C,B.6 JC KIEM_TRA_R_TRAI MOV VUNG,#13 LJMP EXIT_SCAN_PHIM KIEM_TRA_R_TRAI: MOV B,P0 ;01234567 ;10111111 CPL B.0 CPL B.2 MOV C,LEFT2 ORL C,B.0 ORL C,B.2 JC KIEM_TRA_C_PHAI MOV VUNG,#14 LJMP EXIT_SCAN_PHIM KIEM_TRA_C_PHAI: MOV C,LEFT2 ;01234567 ORL C,LEFT3 ;00111111 ORL C,LEFT1 JC KIEM_TRA_C_TRAI MOV VUNG,#15 LJMP EXIT_SCAN_PHIM KIEM_TRA_C_TRAI: MOV C,LEFT3 ;01234567 MOV B,P0 ;01111111 CPL B.1 ORL C,B.1 JC DUNG_LAI MOV VUNG,#16 LJMP EXIT_SCAN_PHIM DUNG_LAI: MOV A,P0 CJNE A,#0FFH,EXIT_SCAN_PHIM MOV VUNG,#17 LJMP EXIT_SCAN_PHIM EXIT_SCAN_PHIM: ACALL DIEU_XUNG_ROBOT RET DIEU_XUNG_ROBOT: DK_R_TIEN: MOV R5,VUNG CJNE R5,#11,DK_R_LUI LCALL STOP_CAMERA SETB RL4 SETB RL5 MOV MOTOR2,V_MOTORL_VT3 MOV MOTOR3,V_MOTORR_VP3 LJMP EXIT_DIEU_XUNG_ROBOT DK_R_LUI: CJNE R5,#12,DK_R_PHAI LCALL STOP_CAMERA CLR RL4 CLR RL5 MOV MOTOR2,V_MOTORL_VT3 MOV MOTOR3,V_MOTORR_VP3 LJMP EXIT_DIEU_XUNG_ROBOT DK_R_PHAI: CJNE R5,#13,DK_R_TRAI LCALL STOP_CAMERA CLR RL4 SETB RL5 MOV MOTOR2,V_MOTORL_VT3 MOV MOTOR3,V_MOTORR_VP3 LJMP EXIT_DIEU_XUNG_ROBOT DK_R_TRAI: CJNE R5,#14,DK_C_PHAI LCALL STOP_CAMERA SETB RL4 CLR RL5 MOV MOTOR2,V_MOTORL_VT3 MOV MOTOR3,V_MOTORR_VP3 LJMP EXIT_DIEU_XUNG_ROBOT DK_C_PHAI: CJNE R5,#15,DK_C_TRAI LCALL STOP ;CHO 2 BANH DUNG LAI NEU KHONG VUA QUAY VUA CUA CLR RL3 MOV MOTOR1,V_MOTORR_VP6 LJMP EXIT_DIEU_XUNG_ROBOT DK_C_TRAI: CJNE R5,#16,DUNG_ROBOT_LAI LCALL STOP ;// SETB RL3 MOV MOTOR1,V_MOTORR_VP6 LJMP EXIT_DIEU_XUNG_ROBOT DUNG_ROBOT_LAI: CJNE R5,#17,EXIT_DIEU_XUNG_ROBOT MOV MOTOR2,#00H MOV MOTOR3,#00H MOV MOTOR1,#00H RET EXIT_DIEU_XUNG_ROBOT: RET CHUONG TRINH SCAN DIEU KHIEN CAMERA DUA VAO TIN HIEU NHAN DC TU MAY TINH========================== EXIT_TRACKING: JNB MAT_THAN_TO,DIEU_XUNG_PWM ACALL DIEU_XUNG_PWM_ROBOT RET ;==== ====>>====================== DIEU_XUNG_PWM: MOV R5,VUNG CJNE R5,#00,DK_VUNG_PHAI_1 JB MAT_THAN_NHO,LUI_LAI1 MOV MOTOR1,#00H ;STOP CAMERA MOV MOTOR2,#00H ;STOP ROBOT MOV MOTOR3,#00H RET DK_VUNG_PHAI_1: CJNE R5,#01,DK_VUNG_LAC_PHAI RET DUNG_CONG_TAC3: LCALL QUAY_TRAI_C LCALL DELAY5 LJMP DUNG_QUAY KT_TIEP: CJNE R5,#02,DUNG_CONG_TAC3 LJMP DUNG_QUAY ;------------------------------------- LUI_LAI1: CLR RL4 ;CHO ROBOT CHAY LUI LAI CLR RL5 MOV MOTOR2,V_MOTORL_VT3 MOV MOTOR3,V_MOTORR_VP3 JB MAT_THAN_NHO,LUI_LAI1 MOV MOTOR2,#00H MOV MOTOR3,#00H RET ;------------------------------------- DK_VUNG_LAC_PHAI: CJNE R5,#02,DK_VUNG_TRAI_1 MOV R2,#50 KT_VUNG_LAC_PHAI: MOV B,P0 ;01234567 ; DJNZ R2,KT_VUNG_LAC_PHAI LJMP XU_LY XU_LY: CLR RL3 MOV MOTOR1,#V_MOTORL_VT2 LCALL SW3 JNB PRO,TIM_VAT_PHAI SJMP DK_VUNG_LAC_PHAI TIM_VAT_PHAI: LCALL STOP_CAMERA LCALL CUA_PHAI LCALL DELAY4 LCALL STOP LJMP CHO_NHAN_VAT_MOI2 CHO_NHAN_VAT_MOI2: LCALL STOP VE_TT2: ;QUAY CAMERA VE GIUA SETB RL3 MOV MOTOR1,#30 LCALL SW2 JNB PRO,DA_VE_TT2 LJMP VE_TT2 DA_VE_TT2: LCALL STOP_CAMERA LJMP DA_VE_TT2 RET CHUYEN: RET ;------------------------------------ DK_VUNG_TRAI_1: CJNE R5,#03,DK_VUNG_LAC_TRAI SETB RL3 MOV MOTOR1,V_MOTORL_VT2 LCALL SW1 JNB PRO,DUNG_CONG_TAC1 RET DUNG_CONG_TAC1: LCALL QUAY_PHAI_C LCALL DELAY5 MOV MOTOR2,#40 MOV MOTOR3,#40 CLR RL4 SETB RL5 K_VUNG_LAC_TRAI: MOV B,P0 ;01234567 ;11011111 CPL B.1 CPL B.3 MOV C,LEFT1 ORL C,B.1 ORL C,B.3 JC DK_VUNG_RA_NGOAI DJNZ R2,KT_VUNG_LAC_TRAI LJMP XU_LY2 XU_LY2: SETB RL3 MOV MOTOR1,#V_MOTORL_VT2 LCALL SW1 JNB PRO,TIM_VAT_TRAI SJMP DK_VUNG_LAC_TRAI TIM_VAT_TRAI: LCALL STOP_CAMERA LCALL CUA_TRAI LCALL DELAY4 LCALL STOP LJMP CHO_NHAN_VAT_MOI DK_VUNG_RA_NGOAI: RET CHO_NHAN_VAT_MOI: LCALL STOP VE_TT: ;QUAY CAMERA VE GIUA CLR RL3 MOV MOTOR1,#30 LCALL SW2 JNB PRO,DA_VE_TT LJMP VE_TT DA_VE_TT: LCALL STOP_CAMERA LJMP DA_VE_TT RET ;============ ============================================= DIEU_XUNG_PWM_ROBOT: MOV R5,VUNG CJNE R5,#00,DK_VUNG_PHAI_ROBOT1 JNB MAT_THAN_TO,KO_DUOI JB MAT_THAN_NHO,LUI_LAI LCALL STOP_CAMERA SETB RL4 ;CHO ROBOT CHAY TOI VAT SETB RL5 MOV MOTOR2,V_MOTORL_VT3 MOV MOTOR3,V_MOTORR_VP3 RET KO_DUOI: MOV MOTOR2,#00H MOV MOTOR3,#00H RET LUI_LAI: CLR RL4 ;CHO ROBOT CHAY LUI LAI CLR RL5 MOV MOTOR2,V_MOTORL_VT3 MOV MOTOR3,V_MOTORR_VP3 JB MAT_THAN_NHO,LUI_LAI MOV MOTOR2,#00H MOV MOTOR3,#00H RET DK_VUNG_PHAI_ROBOT1: CJNE R5,#01,DK_VUNG_TRAI_ROBOT1 LCALL STOP_CAMERA CLR RL5 ;QUAY PHAI ROBOT SETB RL4 MOV MOTOR2,V_MOTORL_VT3 MOV MOTOR3,V_MOTORR_VP3 RET ;------------------------------------ DK_VUNG_TRAI_ROBOT1: CJNE R5,#03,DK_LAC_VAT_TRAI LCALL STOP_CAMERA CLR RL4 ;QUAY TRAI ROBOT SETB RL5 MOV MOTOR2,V_MOTORL_VT3 MOV MOTOR3,V_MOTORR_VP3 LJMP EXIT_DIEU_XUNG_PWM DK_LAC_VAT_TRAI: CJNE R5,#04,DK_LAC_VAT_PHAI LCALL STOP_CAMERA CLR RL4 ;QUAY TRAI ROBOT SETB RL5 MOV MOTOR2,V_MOTORL_VT3 MOV MOTOR3,V_MOTORR_VP3 ;LCALL DELAY4 RET DK_LAC_VAT_PHAI: CJNE R5,#02,EXIT_DIEU_XUNG_PWM LCALL STOP_CAMERA CLR RL5 ;QUAY PHAI ROBOT SETB RL4 MOV MOTOR2,V_MOTORL_VT3 MOV MOTOR3,V_MOTORR_VP3 RET EXIT_DIEU_XUNG_PWM: RET ;==== ==========CAC HAM VAN TOC =========================== TOC_DO_CHAM: ;====== ===VAN TOC CHO BEN TRAI============================= MOV V_MOTORL_VT1,#00 MOV V_MOTORL_VT2,#20 MOV V_MOTORL_VT3,#30 ;0000 ;===== =====VAN TOC CHO BEN PHAI============================= MOV V_MOTORR_VP1,#00 MOV V_MOTORR_VP2,#20 MOV V_MOTORR_VP3,#30 ;000 MOV V_MOTORR_VP6,#30 RET TOC_DO_TRUNG_BINH: ;==== =====VAN TOC CHO BANH TRAI============================= MOV V_MOTORL_VT1,#150 MOV V_MOTORL_VT2,#130 MOV V_MOTORL_VT3,#30 ;0000 MOV V_MOTORL_VT4,#80 MOV V_MOTORL_VT5,#50 MOV V_MOTORL_VT6,#20 MOV V_MOTORL_VT7,#10 MOV V_MOTORL_VT8,#00 ;====== =====VAN TOC CHO BANH PHAI============================= MOV V_MOTORR_VP1,#150 MOV V_MOTORR_VP2,#130 MOV V_MOTORR_VP3,#40 ;000 MOV V_MOTORR_VP4,#80 MOV V_MOTORR_VP5,#50 MOV V_MOTORR_VP6,#30 MOV V_MOTORR_VP7,#10 MOV V_MOTORR_VP8,#00 RET TOC_DO_NHANH: ;======= ==VAN TOC CHO BANH TRAI============================= MOV V_MOTORL_VT1,#210 MOV V_MOTORL_VT2,#150 MOV V_MOTORL_VT3,#120 MOV V_MOTORL_VT4,#100 MOV V_MOTORL_VT5,#80 MOV V_MOTORL_VT6,#50 MOV V_MOTORL_VT7,#20 MOV V_MOTORL_VT8,#10 ;===== ======VAN TOC CHO BANH PHAI========================== MOV V_MOTORR_VP1,#210 MOV V_MOTORR_VP2,#150 MOV V_MOTORR_VP3,#120 MOV V_MOTORR_VP4,#100 MOV V_MOTORR_VP5,#80 MOV V_MOTORR_VP6,#50 MOV V_MOTORR_VP7,#20 MOV V_MOTORR_VP8,#10 RET DOI50ms: MOV R1,#15 LOOP_DOI50MS: MOV R0,#255 DJNZ R0,$ DJNZ R1,LOOP_DOI50ms MOV R0,#00H MOV R1,#00H RET ;============================================================ DELAY800MS: LCALL DOI50ms LCALL DOI50ms RET ;============================================================= DOI1S: MOV R1,#255 LOOP_DOI1S: MOV R0,#255 DJNZ R0,$ DJNZ R1,LOOP_DOI1S RET DELAY4: PUSH 00H MOV R0,#100 MOV TMOD,#01H DL44: MOV TH0,#HIGH(200) MOV TL0,#LOW(200) SETB TR0 JNB TF0,$ CLR TF0 CLR TR0 DJNZ R0,DL44 POP 00H RET DELAY5: PUSH 00H MOV R0,#1 MOV TMOD,#01H DL5: MOV TH0,#HIGH(50) MOV TL0,#LOW(50) SETB TR0 JNB TF0,$ CLR TF0 CLR TR0 DJNZ R0,DL5 POP 00H RET END