BÁO CÁO THỰC TẬP TRUNG TÂM ROBOTICA –ĐẠI HỌC DUY TÂN

MỤC LỤC LỜI MỞ ĐẦU Ngày nay, khoa học cơng nghệ phát triển cách nhanh chóng công nghiệp tự động quan tâm lớn Ứng dụng robot vào phục vụ đời sống ngày nhiều trợ giúp đắc lực lĩnh vực khác Robot dần thay cho người để giảm thiếu sức lao động tăng chất lượng cơng việc.Trong chế tạo robot hay gọi Robocon phong trào bạn sinh viên bạn học sinh yêu thích Đến với chế tạo robot giúp cho biến ý tưởng thành thực Khơi dậy niềm đam mê sáng tạo, từ thiết kế Robot phục vụ cho đời sống người Công nghệ chế tạo robot áp dụng vào thi sáng tao Robocon mà có nhiều ứng dụng thực tế Tuy nhiên, tài liệu Robot chưa phổ biến, chương trình học khơng có mơn học chun sâu robot Bởi chế tạo robot công việc bao gồm nhiều lĩnh vực lập trình, điện, khí Sự kết hợp lĩnh vực lại với chế tạo Robot phục vụ cho công việc, yêu cầu đặt Trong thời gian thực tập Trung tâm Robotica em hiểu thêm nhiều điều công việc chế tạo Robot mà điển hình mơ hình Robocon để dự thi thi sáng tạo Robocon 2014 Chính báo cáo thực tập em xin trình bày số kiến thức thiết kế lập trình robot tự động đơn giản CHƯƠNG 1: GIỚI THIỆU VỀ TRUNG TÂM ROBOTICA –ĐẠI HỌC DUY TÂN 1.1 Chức - Tham mưu cho Ban Giám hiệu Hội đồng quản trị công việc liên - quan đến lĩnh vực robotics Trường Nghiên cứu phát triển, sản xuất sản phẩm dịch vụ phục vụ nhiệm vụ đẩy mạnh ứng dụng khoa học công nghệ hoạt động quản lý đào tạo theo - giai đoạn phát triển nhà Trường Là nơi để sinh viên khoa Công nghệ thông tin, Điện tử viễn thông, Đào tạo quốc tế thực hành công việc liên quan đến việc chế tạo robot, hoạt động đào tạo theo CDIO Là nơi để cán bộ, chuyên viên học tập, nghiên cứu thiết bị, công nghệ 1.2 Nội dung nghiên cứu 1.2.1 Phần cứng - Thiết kế khí (Mục tiêu xây dựng phòng workshop file đính kèm) Thiết kế mạch điện 1.2.2 Phần mềm Kỹ thuật định vị cho Robot - Sử dụng lọc Extended Kamal Filter Kỹ thuật định hướng điều khiển Robot - Bộ điều khiển PID - Bộ điều khiển dựa trường nhân tạo (Artificial Potential Field Navigation) - Bộ điều khiển dựa tọa độ đồ (map) Kỹ thuật xử lý ảnh - Nhận dạng đối tượng (Object Recognition) Edge Detection Camera Calibration Target Tracking using vision (camera) Kỹ thuật thuật tích hợp cảm biến - Tích hợp phần mềm dựa ROS, C++ 1.2.3 Cơ cấu tổ chức - Trung tâm Robotica gồm có: Giám đốc trung tâm Nhóm lập trình VĐK hệ thống nhúng (gồm có thành viên) Nhóm khí (gồm có thành viên) Quản lý xưởng + văn thư (gồm có thành viên) Nhóm cộng tác viên (gồm có thành viên) CHƯƠNG 2: LÝ THUYẾT CƠ BẢN VỀ ROBOT 2.1 Hệ thống cảm biến 2.1.1 Định nghĩa Trong robot Một phần quan trọng robot hệ thống cảm biến - Cảm biến định nghĩa cá hệ thống giác quan robot Robot cần xác định trạng thái mội trường bên (như vạch trắng, màu sắc vật ghi điểm, nơi ghi điểm ) để gửi trạng thái mơi trường đến xử lí đưa phản ứng điều khiển robot để đối phó với kiện bên ngồi - Ví dụ đơn giản nguời Ta ví mắt cảm biến Khi đụng vào vách tường mắt cảm biến truyền liệu cho bộ não Tương tự vậy, cảm biến chuyền liệu cho vi xử lí Sau não gửi trả thông thông phản ứng hướng khác để tránh va đụng vào vách tường Vi xử lí robot thế, gửi thơng tin robot đường 2.1.2 Các loại cảm biến Trong kĩ thuật robocon người ta sử dụng nhiều loại cảm biến là: - Led hồng ngoại (bộ phát thu hồng ngoại) - Quang điện trở (PHOTORESISTANCE): cds cdse - Tế bào quang điện - Cảm biến công nghiệp - Camera số cơng nghệ xử lí ảnh Hình 2.1: Cảm biến quang cảm biến vật thể Các đặc tính IR sensor Trong robot mẫu tự động này, sử dụng linh kiện hồng ngoại (IR): Một linh kiện phát hồng ngoại (IR emitter) linh kiện nhận (IR receiver) tạo thành cặp cảm biến sensor Linh kiện phát tạo tia hồng ngoại sau phản xạ truyền tới linh kiện nhận Hình 2.2: IR Sensor Dòng điện qua linh kiện nhận tỉ lệ với cường độ lượng tia hồng ngoại mà nhận Hình 2.3: Tỉ lệ dòng điện cường độ lượng Trong robot mẫu, linh kiện phát nhận nối mạch theo sơ đồ hình 2.3 Khi TX trạng thái thấp, dòng qua linh kiện phát phát tia hồng ngoại Nếu có vật phản xạ mầu trắng lượng hồng ngoại phản hồi lại tạo dòng cao qua linh kiện nhận, điện áp IRS0 hạ xuống Nếu vật phản xạ có mầu đen, mầu sẫm điện áp IRS0 nâng lên Hình 2.4: Sơ đồ nguyên lý mạch phân biệt vạch dẫn đường IR Sensor điều biến Độ xác sensor hồng ngoại miêu tả phần trước bị ảnh hưởng nhiều ánh sáng môi trường Mức điện áp đầu sensor IR không phụ thuộc vào cường độ ánh sáng phản xạ mà bị thay đổi điều kiện ánh sáng môi trường Để giảm bớt ảnh hưởng ánh sáng môi trường, ta dùng sensor hồng ngoại điều biến thay sensor IR thông thường Trong hệ thống sensor IR điều biến, linh kiện phát bật tắt đồng hồ Tín hiệu nhận sau qua mạch khuếch đại chiều DC hình vẽ Tụ C2 mạch chặn dòng DC khỏi tín hiệu qua khuếch đại Chỉ có phần xoay chiều tín hiệu thay đổi tương ứng với tia hồng ngoại phản xạ phát từ phát khuếch đại tích hợp vào mức chiều đầu mạch Như mức mạch phụ thuộc vào tia hồng ngoại mà không phụ thuộc vào ánh sáng mơi trường Hình 2.5: Sơ đồ ngun lý mạch điều biến 2.2 Hệ thống truyền động 2.2.1 Định nghĩa Hệ thống truyền động ví chân, tay robot Robot di chuyển, cầm, nắm, đuợc có hệ thống truyền động Hệ thống truyền động bánh xe, động bao gồm động bước (Stepper Motor) động chiều (DC Motor) động trợ động (Servo Motor)… ; dây truyền động: dây cuaroa, dây xích (xên nhơng dĩa), dây kéo, ròng rọc vật dụng khí khác 2.2.2 Các loại động Có loại động : - Động chiều (DC Motor) - Động bước (Stepper Motor) - Động trợ động (Servo Motor)  Động chiều (DC Motor) Hình 2.6: Động Planet Động chiều có cấu tạo từ hai phận: Bộ phận đứng yên cấu tạo từ nam châm vĩnh viễn đuợc gọi Stato phận quay cấu tạo từ nam châm nhận tạo có cấu tạo từ cuộn dây đồng có lõi kim loại đuợc gọi Roto Động xoay theo chiều thuận chiều nghịch kim đồng hồ Có gắn thêm Encoder để đếm số vong xoay động Hình 2.7: Động quay theo chiều kim đồng hồ Xét hoạt động DC motor: • Như hình ta thấy cho dòng diện chiều (tuyệt đối ko dùng điện xoay chiều) qua motor khiến cho motor quay hình Hình 2.8: Động quay theo chiều ngược kim đồng hồ Khi ta đảo hai cực dòng điện ta sẽ đuợc tượng hình trên, động quay theo chiều hồn tồn ngược lại so với hình truớc Qua ta rút đuợc kết luận sau: - Động DC cho dòng diện chiều chạy qua làm quay phần Roto dẫn đến motor quay, ta đổi chiều dòng điện motor quay theo chiều ngược lại Do ta điều chỉnh hướng quay DC Motor Đặc tính kĩ thuật hầu hết động DC tốc độ quay (vòng/phút) cao moment ngẫu lực thấp Nhưng mà hầu hết robot cần tốc độ quay chậm moment ngẩu lực cao Do hộp số lắp vào DC Motor nhằm giảm tốc độ quay tăng moment ngẫu lực Trên hộp số thường ghi rõ số truyền động trước sau hộp số, ví dụ tốc độ quay DC Motor 1000 vòng/phút đuợc gắn vào hộp số 1000:1 Có nghĩa tốc độ quay 10 đầu hộp số đuợc 1/1000 tốc độ DC Motor chưa gắn hộp số Vận tốc hộp số = Vận tốc DC Motor/1000 = 8000/1000 = vòng/phút Chú ý: Điện áp V dòng I khơng nên vượt giá trị ghi motor, không motor bị hỏng Tuy nhiên ta đặt điện áp thấp V dòng điện thấp I, tốc độ độ chịu tải motor giảm theo - Điều khiển chiều quay DC Motor: Trong phần ta xét cách điều khiển phổ biến đa số đuợc đội Robocon sử dụng • Điều khiển chiều DC Motor mạch cầu H: Mạch cầu H nhiều đội thi Robocon sử dụng tỉnh đơn giản nó, có tên mạch cầu H sơ đồ có hình chữ H: Hình 2.9: Mạch cầu H Trước hết ta xét trường hợp xoay thuận DC Motor, ta lấy ví dụ: Trong đoạn mạch đây, tưởng tượng đoạn mạch cầu xem transistor Q1, Q2, Q3, Q4 khúc cầu bị gãy, người ta bắt đầu từ nguồn điện đến mass, cầu bị gãy người ta khơng thể qua đựơc người ta khơng thể qua điển Q1, Q2, Q3, Q4 Khi mà nhà nước nghe tin cầu gẫy cho kĩ sư sữa cầu đến sữa cầu đường “thuận” theo đường “thuận” sữa điểm Q1, Q4.Sau sữa xong cầu liền lại điểm Q1,Q4 người 14 Hình 2.14: Bên động bước Động bước làm việc nhờ chuyển mạch điện tử đưa tín hiệu điều khiển vào Stator theo thứ tự định tần số định Tổng số góc quay Rotor tương ứng với số lần chuyển mạch, chiều quay tốc độ quay Rotor phụ thuộc vào thứ tự chuyển đổi tần số chuyển đổi Nếu xét phương diện dòng điện, xung điện áp đặt vào cuộn dây Stator (phần ứng) động bước, Rotor (phần cảm) động quay góc định, góc bước quay động Ở ta định nghĩa góc bước (Step Angle) độ quay nhỏ bước nhà sản xuất quy định Khi xung điện áp đặt vào cuộn dây phần ứng thay đổi liên tục Rotor quay liên tục (thực chất chuyển động theo bước rời rạc) Hình 2.16: Cấu tạo động bước 15 Theo phương diện khác, coi động bước linh kiện hay thiết bị số (Digital Device) mà thơng tin số hoá thiết lập chuyển thành chuyển động quay theo bước Động bước thực trung thành lệnh số hóa mà máy tính u cầu Hình 2.16: Một xung tương ứng với bước rôto (1 xung – bước) Hình 2.17: Mơ tả tương quan q trình điện trình động bước Nguyên lý làm việc chung động bước Khác với động đồng thông thường, Rotor động bước khơng có cuộn dây khởi động mà khởi động phương pháp tần số, Rotor động bước kích thích (Rotor tích cực) khơng kích thích (Rotor thụ động) Xung điện áp cấp cho cuộn dây Stator xung cực cực: 16 Hình 2.18: Xung điện áp cấp cho cuộn dây Stator Chuyển mạch điện tử cung cấp điện áp điều khiển cho cuộn dây Stator theo cuộn riêng lẻ, theo nhóm cuộn dây Trị số chiều lực điện từ tổng F phụ thuộc vào vị trí lực điện từ thành phần Do vị trí Rotor động bước khơng gian, hồn toàn phụ thuộc vào phương pháp cung cấp điện cho cuộn dây: Hình 2.19: Sơ đồ nguyên lý động bước m pha với Roto cực lực điện từ điều khiển xung cực 17 Hình 2.19 vẽ sơ đồ nguyên lý động bước m pha với Rotor có cực (2p=2) khơng kích thích Nếu cuộn dây động bước cấp điện cho cuộn dây riêng lẻ theo thứ tự 1, 2, 3,… m, xung cực, Rotor động bước có m vị trí ổn định trùng với trục cuộn dây (hình 2.19a) Để tăng cường lực điện từ tổng Stator tăng từ thơng moment đồng bộ, ta cấp điện đồng thời cho hai, ba nhiều cuộn dây Lúc Rotor động bước có vị trí cân (ổn định) trùng với vector lực điện từ tổng F Đồng thời lực điện từ tổng F có giá trị lớn lực điện từ thành phần cuộn dây Stator (hình 2.19b, 2.19c) Hình 2.19b vẽ lực điện từ tổng F cung cấp điện đồng thời cho số chẵn cuộn dây (2 cuộn dây) Lực điện từ tổng F có trị số lớn nằm vị trí hai trục hai cuộn dây Hình 2.19c vẽ lực điện từ tổng F cung cấp điện đồng thời cho số lẻ cuộn dây (3 cuộn dây) Lực điện từ tổng F nằm trùng với trục cuộn dây có trị số lớn Tóm lại, hai trường hợp cấp điện cho số chẵn cuộn dây cấp điện cho số lẻ cuộn dây, Rotor động bước có m vị trí cân Góc xê dịch hai vị trí liên tiếp Rotor 2π/m Nếu cấp điện theo thứ tự số chẵn cuộn dây, số lẻ cuộn dây (ví dụ, kết hợp hình 2.19b, 2.19c), hay nghĩa số lượng cuộn dây điều khiển luôn thay đổi từ chẵn sang lẻ ngược lại, số vị trí cân Rotor tăng lên gấp đơi 2m, độ lớn bước giảm nửa 2π/m Trường hợp gọi điều khiển không đối xứng, hay điều khiển nửa bước (half step) Nếu số lượng cuộn dây điều khiển luôn không đổi (một số chẵn cuộn dây số lẻ cuộn dây, ví dụ hình 2.19b hình 2.19c) Rotor có m vị trí cân gọi điều khiển đối xứng, hay điều khiển bước (full step)  Động trợ động (Servo Motor) Động servo sử dụng để kiểm sốt chuyển động loạt ngành cơng nghiệp điện, từ robot để sản xuất CNC công nghệ hàng không vũ trụ Động servo phần hệ thống khép kín, gọi “một hệ thống động servo”, mà không sử dụng động bước Hệ thống động servo bao gồm số phận cụ thể mạch điều khiển, động servo, trục, chiết áp, ổ bánh 18 (tùy thuộc vào loại động servo), khuếch đại endocer Động servo phải có khả năng: • Hoạt động loạt tốc độ khơng q nóng; • Hoạt động tốc độ số giữ đủ mô-men xoắn giữ tải vị trí; • Vận hành tốc độ thấp thời gian dài với mức nóng Ba loại động servo sử dụng hệ thống động servo là: • Động servo AC (dựa thiết kế động cảm ứng) • Động DC servo (dựa thiết kế động chiều) • Động servo không chổi than (dựa thiết kế động đồng bộ) Như phần chuyển động liên tục, servo motor thường bị phá vỡ cần phải sửa chữa Trong ngành công nghiệp sản xuất, động servo bị bắn rơi phá vỡ sản xuất lập kế hoạch thỏa hiệp Trong kinh tế ngày nay, bạn đủ khả để phía sau tiến độ bỏ lỡ thời hạn Điều Xung PWM, điều chỉnh vận tốc cho động Điều xung PWM (Pulse With Moduration) khâu quan trọng thiết kế Robocon Qua bạn điều khiển tốc độ linh hoạt cho DC Motor từ dùng hàm vận tốc robot bám theo vạch trắng mà Điều xung nói chung cách bật tắt nguồn điện Motor liên tục làm Motor lúc nhận đựơc điện lúc điện Khi tần số đủ cao Motor quay ổn định tốc độ nhờ moment quay bánh xe Xung vuông đồ thị biểu trạng thái nguồn điện Nguồn mở Motor hoạt động, lúc xung vng trạng thái cao nguồn tắt Motor dừng lúc xung vuông trạng thái thấp Tốc độ motor phụ thuộc vào dộ dài xungvuông bạn xem hình biểu thị xung vng dứơi gồm: Xung 20%, xung 50% xung 80% xem khác biệt Hình 2.20: Xung vng PWM 19 Ta xem xung trên, xung dù 20% 50% hay 80% có trạng thái 12V 0V, 12V đựơc gọi trạng thái cao trạng thái Motor quay, 0V trạng thái thấp trạng thái Motor có có điện áp dừng Trong biểu đồ xung, tổng % trang thái cao trạng thái thấp 100% số xung 20%, 50% 80% xung cao Cho nên % xung thấp = 100% - Xung cao Nếu xung cao dài tốc độ Motor cao, ta có Motor đạt đựơc tốc độ cao motor gửi đến xung 100% Motor dừng có có xung 0% đựơc gửi tới Biện pháp điều xung cho motor: Chúng ta điều xung cho Motor dùng vi điều khiển có hỗ trợ kênh điều xung mà thơi Các vi điều khiển có kênh điều xung có nhiều Pic16F877A P89C51RD Philip 2.3 Bánh xe đa hướng Hình 2.21: Bánh xe Omi Mecanum Hình 2.22: Robot đa hướng dùng Mecanum 20 - Bánh số lăn bố trí phía giống nhau, số giống lăn quay phía ngược lại so với bánh số (hình) - Khi bố trí bánh mecanum tất lăn bánh xe hướng vào theo chiều mũi tên (hình) Khi di chuyển lập trình: + Đi thẳng: Bánh 1, 2, 3, quay thuận chiều kim đồng hồ (hình) đế thẳng phía trước + Đi lùi: Bánh 1, 2, 3, quay ngược chiều kim đồng hồ (hình) đế lùi phía sau + Sang ngang qua trái: Bánh 1, quay ngược chiều kim đồng hồ (hình), bánh 3, quay thuận chiều kim đồng hồ (hình) + Sang ngang qua phải: Bánh 1, quay thuận chiều kim đồng hồ (hình), bánh 3, quay ngịch chiều kim đồng hồ (hình) + Sàngchéo theo hướng mũi tên bánh số 1: Bánh 3, ghì (dừng), bánh 1, quay nghịch chiều kim đồng hồ (hình) + Sang chéo theo hướng mũi tên bánh số 2: Bánh 3, ghì (dừng), bánh 1, quay thuận chiều kim đồng hồ (hình) + Sang chéo theo hướng mũi tên bánh số 3: Bánh 1, ghì (dừng), bánh 3, quay thuận chiều kim đồng hồ (hình) + Sang chéo theo hướng mũi tên bánh số 4: Bánh 1, ghì (dừng), bánh 3, quay nghịch chiều kim đồng hồ (hình) + Bẻ lái sang phải: Bánh 1, quay thuận chiều kim đồng hồ, bánh 2, quay nghịch chiều kim đồng hồ + Bẻ lái sang trái: Bánh 1, quay nghịch chiều kim đồng hồ, bánh 2, quay thuận chiều kim đồng hồ Trong hình bánh mecanum dùng kỹ thuật đồng trục (động bánh xe quay trục) + Ưu điểm: Ko bị chênh lệnh tốc độ quay động bánh xe + Nhược điểm: phải có board driver tốt, ko di chuyển lâu FET cháy, giảm tuổi thọ FET, Ngoài ra, bánh mecanum dùng kỹ thuật truyền sin (1 buli trung gian buli trung gian) + Ưu điểm: Khi di chuyển động quay nhiều vòng bánh xe (giống nguyên lý xích xe đạp), trục động truyền gián tiếp qua dây sin buli nên chạy bánh xe không tác động lực trực tiếp vào động => Ít hại FET so với chuyển động đồng trục 21 + Nhược điểm: Khi thiết kế khoảng cách buli trung gian trục động cơ, trục bánh xe bị chênh lệch, ko xác tuyệt đội bánh xe, chạy bánh xe ko 2.4 Encoder cho bánh xe Enconder hệ thống gồm cặp thu, phát hồng ngoại đĩa tròn có rãnh Encoder dùng để quản lí xác định vị trí góc quay vật chuyển động tròn bánh xe, trục động mà ứng dụng encoder kĩ thuật robocon đếm số vòng quay bánh xe Hình 2.23: Một đĩa tròn encoder vòng lỗ Vì robot ứng dụng encoder đếm vòng quay động nên đề cập đến encoder đếm lên (Incremental encoder) Trong Encoder độ phân giải số lỗ vòng, ví dụ đĩa encoder có lỗ/vòng Cách thức hoạt động khơng có phức tạp Encoder sử dụng cặp led phát, thu hồng ngoại Các led hồng ngoại đựơc đặt đối diện nhau, led ta đặt đĩa encoder vào Hình 2.24: Encoder đặt hai led hồng ngoại Như hình bạn thấy, bánh xe chuyển động đĩa quay chuyển động 22 đĩa đứng sử dụng để thu nhỏ tia sáng thôi, không nên ý đến đĩa đứng làm Trên đĩa quay có rãnh (lỗ) quay đến led chiếu vào rãnh tia hồng ngoại đuợc chiếu xuyên qua rãnh đến led thu, led chiếu vào chỗ khơng có rãnh đĩa ánh sáng qua đĩa Cho nên nhận đươc tín hiệu biết led điqua rãnh Vậy để biết đĩa quay vòng? Rất đơn giản, giả sử có đĩa encoder có vòng lỗ và vòng lõ có lỗ Khi bắt đầu ta led chiếu qua vòng lỗ cho encoder quay Khi quay đủ vòng quay lại lỗ led chiếu vào lỗ truyền tín hiệu quay hồn tất vòng, lỗ gửi tín hiệu xung vng - Encoder có vòng nhiều lỗ có độ phân giải cao (xung/vòng) xử lí bạn phải có tộc độ thật cao để nhận biết kịp thời xung tốc độ động cao.do với ứng dụng bình thường ta nên dùng vòng encoder lộ để đếm Ví dụ có đĩa encoder 16 lỗ vòng Chúng ta lập trình đếm (Counter) ngồi để đếm số lỗ, đếm lên đến 16 lỗ sử dụng ngắt để báo đông hồn tất vòng quay - Trong trường hợp có tác động khiến cho encoder đếm sai lỗ, đếm sai lỗ số lớn Nhưng trường hợp ta muốn động quay nhiều vong sai lỗ tích luỹ số lớn Cho nên tạo theo lỗ định vị bên võng lỗ đếm nữa: Hình 2.25: Tạo lỗ vòng lỗ đếm Khi led lỗ định vị báo motor hồn tất 1vòng quay mà vong ngồi chưa có tín hiệu hồn tất vòng quay cho lỗi đếm chấp nhận vòng quay - Làm để biết chiều quay đông cơ? Ta sử dụng thêm cặp led phát 23 thu cặp đặt lệch so với cặp thứ 1, ví dụ cặp thứ chiếu vào rãnh cặp khơng chiếu vào rãnh Do rãnh quay có sơ đồ xung hình sau: Hình 2.26: Sơ đồ xung rãnh quay Các bạn thấy xung A từ mức cao xuống mức thấp, mà lúc B mức thấp, xác định chiều chuyển động encoder theo chiều mũi tên màu cam Nếu A từ mức cao xuống mức thấp, mà B mức cao, biết encoder quay theo chiều màu đen 24 CHƯƠNG 3: LẬP TRÌNH MỘT ROBOT TỰ ĐỘNG 3.1 Yêu cầu Lập trình robot tự động với yêu cầu có: Khả lần theo vạch trắng Khả nhận biết đếm điểm giao qua 3.2 Lần theo vạch trắng Trong robot tự động mẫu chúng tôi, đặt cặp sensor IR: Bên trái (Left) bên phải (Right) phần đầu robot Khi sensor trái cắt đường trắng, giá trị sensor hạ xuống mức điện áp ngưỡng đặt trước buộc phải robot chạy sang trái vài bước để điều chỉnh hướng Hoặc sensor phải cắt vào vạch trắng, giá trị sensor phải thấp giá trị ngưỡng đặt sẵn, phải cho robot chạy sang phải vài bước để điều hướng chạy xác Nếu hai sensor khơng cắt vạch trắng giả định hướng để chạy thẳng 25 Hình 3.1: Thuật tốn lần theo vạch trắng 3.3 Dò đếm đường cắt Để dò điểm giao vach robot chạy qua, kiểm tra sensor trái phải Nếu hai sensor cắt vạch trắng lúc cho robot qua điểm giao Trong trường hợp này, phải robot tiếp vài bước để tránh đếm lần điểm giao Hình 3.2: Thuật tốn nhận biết đếm điểm giao 3.4 Chương trình Hàm C ví dụ cho robot lần theo đường trắng đếm vạch giao qua: …… void move_robot(unsigned char step) { int cnt; cnt=0; while(cntALIGN_THRESHOLD && read_sensor(2)>ALIGN_THRESHOLD) movet(FORWARD,1,0); 26 else if (read_sensor(1) ALIGN_THRESHOLD) movet(TURN_LEFT,1,80); else if (read_sensor(2) ALIGN_THRESHOLD) movet(TURN_RIGHT,1,80); else if (read_sensor(2)