kl do ngoc dung 710015d

GVHD:Th.s Hồng Trung Hiếu Thiết kế, thi cơng điều khiển Robot Chương TÌM HIỂU VỀ ROBOT I mục đích: Mục đích thiết kế , thi công và điều khiển Robot để tham dự cuộc thi ROBOCON đài truyền hình tổ chức Trong cuộc thi này Robot chạy nền sân sau: Figure 1-Hình sân thi đấu Trên sân có hai độ i, một đội đỏ và một đội xanh Hai đội cùng lấy quà và đặt quà vào vị trí cố định để ghi thật nhiều điểm Sau ba phút đội nào ghi nhiều điểm thì đội đó thắng SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page GVHD:Th.s Hoàng Trung Hiếu Thiết kế, thi công điều khiển Robot Giới thiệu về sân: Có vùng cho Robot bằng tay(manual machine area)  Có vùng cho Robot tự đợng(autonomous machine area)  Có vùng chung cho Robot bằng tay và Robot tự động(common area) II  Yêu cầu: Để thiết kế Robot ta phải theo luật của nó, cuộc thi này cũng vậy Em xin giới thiệu sơ lược vài ý sau:  Khối lượng của tất cả Robot không 50 kg  Khi xuất phát tất cả Robot phải đặt diện tích 1m2  Chiều cao tối đa vùng khỡi động là 1m  Khi Robot khỡi động chiều dài vương không quá 1m35  Khối quà gần cao 75 cm  Khối quà trung tâm cao 1m5 Chú ý:  Là ở khối quà trung tâm , phải có hai Robot tự động cỡi để lấy quà để không bị phạm lỗi Hoặc Robot bằng tay nâng Robot tự động để lấy quà  Khi thi đấu không làm hư mặt sân thi đấu  Không cố tình làm hư Robot đội bạn SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page GVHD:Th.s Hoàng Trung Hiếu Thiết kế, thi công điều khiển Robot Chương GIỚI THIỆU THIẾT BỊ Động DC: I Thành phần quan trọng Robot truyền động, phần tử tạo nên truyền động động Động tạo nên sức mạnh độ xác cho Robot bạn Vậy bạn chọn động cho phù hợp? Mỗi loại động có một ưu điểm riêng vì thế mà ta có ứng dụng riêng cho mỗi loại động Động không có hộp giảm tốc thường tốc độ lớn và moment yếu Để đưa vào ứng dụng thì bạn cần giảm tốc độ qua bộ giảm tốc hoặc đai Figure 2-Động DC không giảm tốc SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page GVHD:Th.s Hồng Trung Hiếu Thiết kế, thi cơng điều khiển Robot Figure 3-Hình động DC không giảm tốc Động có giảm tốc thì moment lớn tốc độ thường không lớn, vấn đề là bạn phải chọn động nào cho phù hợp tốc độ và moment Figure 4-Động DC có hộp giảm tốc Động servo DC thường không có hộp giảm tốc có hồi tiếp về bằng xung bạn có thể giảm tốc xuống để sử dụng để tăng moment , bạn có thể điều khiển động theo tốc độ là tốt nhất Figure 5-Động DC servo Ngoài còn có nhiều loại khác nữa , ba loại là thường có nhiều thị trường chúng ta Tức nhiên là chúng ta thường mua các loại động đã qua sử dụng SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page GVHD:Th.s Hoàng Trung Hiếu Thiết kế, thi công điều khiển Robot Sau là cách gắn động vào: Figure 6-Cách gắn động Đó chỉ là một nhữ ng cách thông thường hay gắn động Ngoài tùy thuộc vào trường hợp mà ta có những cách gắn động riêng II Cảm biến: Sử dụng cảm biến công nghệp hãng Keyency, là loại thu phát hồng ngoại bốn đầu  Input(Brown) : 12 – 24 V  GND(Blue): 0V  Output(Black):0_40 V, 100mA max  Stabily(Orange): 0- 40V, 50mA max Chú ý là ngõ của cảm biến là Transistor , đó mình cần kéo nguồn cho ngõ Như biễu diễn hình bên dưới SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page Thiết kế, thi công điều khiển Robot GVHD:Th.s Hoàng Trung Hiếu 12V VCC R4 R sensor1 R3 R ISO1 output sensor PC817 Figure 7-Cách sử dụng cảm biến Ngõ đọc tín hiệu về đưa vào vi xử lý chính là tín hiện sensor sau đã chuyển áp về 5V qua opto PC817 III Giới thiệu vi điều khiển PIC 18F4431 TÍNH NĂNG a Modun điều khiển PWM 14 bit  kênh đôi với ngõ bù  Hổ trợ thao tác cạnh mức  Ngõ vào bảo vệ lỗi phần cứng  Cập nhận đồng thời chu kỳ nhiệm vụ chu kỳ  Linh hoạt với ngõ kích khởi kiện b Modun motion feedback  Ba kênh chốt/bắt giữ ngõ vào riêng biệt  Chế độ vận hành linh hoạt cho ứng dụng đo chu kỳ độ rộng xung  Modun giao diện cảm biến Hall  Tính kích khởi kiện đến modun khác  Giao diện Quadrature encoder  Hai ngõ vào phase ngõ vào index từ encoder  Cho phép theo dõi mức cao/thấp, trạng thái chiều thay đổi hướng ngắt SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page GVHD:Th.s Hoàng Trung Hiếu Thiết kế, thi công điều khiển Robot  Phép đo vận tốc c Bộ chuyển đổi ADC 10 bit 200ksps, tốc độ cao  kênh ngõ vào  Lấy mẫu đồng thời kênh ngõ vào  Lấy mẫu 1,2 kênh  Khả chuyển đổi tự động  word FIFO cho phép lựa chọn tần số ngắt  Sự lựa chọn kích khởi từ bên ngồi  Thời gian nhận lập trình d Cấu trúc dao động linh hoạt  kiểu dao động tinh thể đến 40Mhz  modun xung clock đến 40Mhz  Khối dao động nội + Cho phép người sử dụng lựa chọn tần số từ 31Khz đến 8Mhz + OSCTUNE bù trơi tần số  Nguồn dao động phụ sử dụng cho timer tần số 31Khz  Dò tự phục hồi xung clock + Tắt nguồn thiết bị xung clock bị lỗi  Chế độ quản lý nguồn +Run: CPU on, ngoại vi on +Idle: CPU off, ngoại vi on +Sleep: CPU off, ngoại vi off +Bộ dao động hai tốc độ khởi động e Những điểm mạnh ngoại vi  Dòng ngõ cao 25mA  Ba ngõ vào ngắt  Hai modun bắt giữ, so sánh, điều biến độ rộng xung + Chế độ capture 16 bit, độ phân giải tối đa 6.25 ns + Chế độ compare 16 bit, độ phân giải tối đa 100 ns + Ngõ PWM, PWM độ phân giải từ đến 10 bit  Bổ sung module USART SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page GVHD:Th.s Hoàng Trung Hiếu Thiết kế, thi công điều khiển Robot + Hỗ trợ RS_485, RS_232 LIN 1.2 +Tự động thức dậy bit start o Chế độ dò tự động  RS_232 hoạt động dùng xung clock nội  Các tính đặc biệt vi điều khiển  Tăng cường nhớ chương trình Flash với 100.000 chu kỳ đọc ghi  Bộ nhớ liệu EEPROM với 1.000.000 chu kỳ đọc ghi  Bộ nhớ Flash liệu EEPROM trì 100 năm  Tự lập trình với phần mềm điều khiển  Chế độ ngắt mức ưu tiên  Mở rộng timer Watchdog: chu kỳ lập trình từ 41ms đến 131s  Lập trình sử dụng nguồn đơn mạch Figure 8-Sơ đồ chân của PIC18F4431 CẤU HÌNH CÁC BỘ DAO ĐỘNG a Các loại dao động PIC 18F4431 hoạt động 10 chế độ dao động khác Người sử dụng lập trình cấu hình bit F0SC3:F0SC0 ghi cấu hình 1H để lựa chọn 10 chế độ  LX Dao động tinh thể nguồn thấp SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page GVHD:Th.s Hồng Trung Hiếu Thiết kế, thi cơng điều khiển Robot  XT Tinh thể/bộ cộng hưởng  HS Tinh thể/bộ cộng hưởng tốc độ cao  HSPLL Tinh thể/bộ cộng hưởng tốc độ cao với cho phep PLL  RC Dao động tụ điện/điện trở bên với Fosc/4 ngõ chân RA6  RCIO Dao động tụ điện/điện trở bên với I/O chân RA6  INTIO1 Bộ dao động nội với Fosc/4 ngõ chân RA6 I/O chân RA7  INTIO2 Bộ dao động nội với Fosc/4 ngõ chân RA6 RA7  EC Xung clock bên với ngõ Fosc/4  ECIO Xung clock bên với I/O RA6 b Bộ dao động tinh thể/cộng hưởng  Chân 13(OSC1) và chân 14(OSC2)  Trong chế độ dao động XT,LP,HS HSPLL tinh thể cộng hưởng gốm kết nối vào chân OSC1 OSC2 để thiết lập dao động SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page GVHD:Th.s Hồng Trung Hiếu Thiết kế, thi cơng điều khiển Robot Cấu hình XT,LP,HS HSPLL Table 1-giá trị tụ điện cho bợ cợng hưởng gớm Gía trị tụ điện cho cộng hưởng gốm Table 2-giá trị tụ cho bộ giao động tinh thể Giá trị tụ điện cho dao động tinh thể c Khối dao động nội PIC 18F4431 có sẵn khối dao động nội, tạo hai tín hiệu clock khác sử dụng nguồn xung clock hệ thống Khối cho phép loại bỏ dao động bên ngồi chân OSC1 OSC2 Ngõ (INTOSC) nguồn xung clock 8MHz sử dụng trực tiếp để cung cấp cho xung clock hệ thống Nếu sử dụng chia cung cấp dãy tần số từ 125KHz đến 4MHz để lựa chọn Ngõ INTOSC cho phép tần số xung clock hệ thống từ 125KHz đến 8MHz lựa chọn Ngồi cịn có nguồn dao động nội RC (INTRC) cung cấp tần số 31KHz Dao động INTRC cho phép việc lựa chọn khối dao động nội nguồn xung clock hệ thống theo sau cho phép:  Power_up timer  Fail_safe Clock Monitor SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page 10 GVHD:Th.s Hoàng Trung Hiếu Thiết kế, thi công điều khiển Robot output_low(pin_b2); delay_ms(30); kiem_tra1: sensor=input_a(); sensor=sensor&0x3f; sensor_right=sensor&0x01; write_duty_pwm0(duty_cycle=pwm); write_duty_pwm1(duty_cycle=pwm); if(sensor_right!=0x01) goto kiem_tra1; kiem_tra2: sensor=input_a(); sensor=sensor&0x3f; sensor_left=sensor&0x20; write_duty_pwm0(duty_cycle=pwm); write_duty_pwm1(duty_cycle=pwm); if(sensor_left!=0x20) goto kiem_tra2; write_duty_pwm1(duty_cycle=0); write_duty_pwm0(duty_cycle=0); output_high(pin_b2); output_high(pin_b0); delay_ms(30); old_sensor=sensor_left; } /******************************************************************/ ////////////////////////CHUONG TRINH CUA PHAI////////////////////////// /******************************************************************/ //da kem tra //chuong trinh su dung cua phai = cach cho hai dong co quay nguoc chieu //co ham dong lai sau thuc hien cua xong SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page 71 GVHD:Th.s Hồng Trung Hiếu Thiết kế, thi cơng điều khiển Robot //sau thoat chuong trinh thi cho hai dong co chay tien //nhan biet dung line bang cach kiem tra sensor phai va trai void cua_phai(int16 pwm) { int8 sensor,sensor_right,sensor_left; sensor=input_a(); sensor=sensor&0x3f; sensor_right=sensor&0x01; sensor_left=sensor&0x20; write_duty_pwm0(duty_cycle=0); write_duty_pwm1(duty_cycle=0); output_low(pin_b0); output_high(pin_b2); delay_ms(100); kiem_tra3: sensor=input_a(); sensor=sensor&0x3f; sensor_left=sensor&0x20; write_duty_pwm0(duty_cycle=pwm); write_duty_pwm1(duty_cycle=pwm); if(sensor_left!=0x20) goto kiem_tra3; sensor=input_a(); sensor=sensor&0x3f; sensor_right=sensor&0x01; kiem_tra4: sensor=input_a(); sensor=sensor&0x3f; sensor_right=sensor&0x01; write_duty_pwm0(duty_cycle=pwm); write_duty_pwm1(duty_cycle=pwm); SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page 72 GVHD:Th.s Hoàng Trung Hiếu Thiết kế, thi công điều khiển Robot if(sensor_right!=0x01) goto kiem_tra4; write_duty_pwm0(duty_cycle=0); write_duty_pwm1(duty_cycle=0); output_low(pin_b2); output_high(pin_b0); delay_ms(50); output_high(pin_b0); outPut_high(pin_b2); delay_ms(50); old_sensor=sensor_right; } /******************************************************************/ /////////////////////////CHUONG TRINH CON DO LINE ENCODER///////////////// /******************************************************************/ //bien xung 16 bit //da kiem tra chuong trinh su dung timer //nhap so xung can chay vao do_line_encoder(xung=so xung can nhap) //khong cho phep ngat toan cuc void do_line_encoder(int16 xung) { int8 sensor,old_sensor; t1con=0x02;//ti le xung la 1:1 tmr1ie=0;//khong cho phep ngat timer tmr1if=0; xung=65535-xung; tmr1_l=make8(xung,0); tmr1_h=make8(xung,1); tmr1on=1; //doc sensor sensor=input_a(); SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page 73 GVHD:Th.s Hoàng Trung Hiếu Thiết kế, thi công điều khiển Robot sensor=sensor&0x3f; old_sensor=sensor; while(tmr1if==0) { sensor=input_a(); sensor=sensor&0x3f; switch (sensor) { case 0x08: { if(old_sensor==0x00) { write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=150); } else if(old_sensor==0x18) { write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=250); } else { write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=200); } old_sensor=sensor; break; } case 0x04: { if(old_sensor==0x00) SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page 74 GVHD:Th.s Hồng Trung Hiếu Thiết kế, thi cơng điều khiển Robot { write_duty_pwm0(duty_cycle=150); write_duty_pwm1(duty_cycle=300); } else if(old_sensor==0x06) { write_duty_pwm0(duty_cycle=200); write_duty_pwm1(duty_cycle=300); } else { write_duty_pwm0(duty_cycle=100); write_duty_pwm1(duty_cycle=300); } old_sensor=sensor; break; } case 0x18: { if(old_sensor==0x08) { write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=50); } else if(old_sensor==0x10) { write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=200); } else { SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page 75 GVHD:Th.s Hoàng Trung Hiếu Thiết kế, thi công điều khiển Robot write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=100); } old_sensor=sensor; break; } case 0x30: { if(old_sensor==0x20) { write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=100); } else if(old_sensor==0x10) { write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=50); } else { write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=0); } old_sensor=sensor; break; } case 0x20: { if(old_sensor==0x00) { write_duty_pwm0(duty_cycle=300); SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page 76 GVHD:Th.s Hồng Trung Hiếu Thiết kế, thi cơng điều khiển Robot write_duty_pwm1(duty_cycle=0); } else if(old_sensor==0x30) { write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=50); } else { write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=0); } old_sensor=sensor; break; } case 0x06: { if(old_sensor==0x04) { write_duty_pwm0(duty_cycle=100); write_duty_pwm1(duty_cycle=300); } else if(old_sensor==0x02) { write_duty_pwm0(duty_cycle=200); write_duty_pwm1(duty_cycle=300); } else { write_duty_pwm0(duty_cycle=150); write_duty_pwm1(duty_cycle=300); SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page 77 GVHD:Th.s Hoàng Trung Hiếu Thiết kế, thi công điều khiển Robot } old_sensor=sensor; break; } case 0x03: { if(old_sensor==0x02) { write_duty_pwm0(duty_cycle=100); write_duty_pwm1(duty_cycle=300); } else if(old_sensor==0x01) { write_duty_pwm0(duty_cycle=150); write_duty_pwm1(duty_cycle=300); } else { write_duty_pwm0(duty_cycle=50); write_duty_pwm1(duty_cycle=300); } old_sensor=sensor; break; } case 0x01: { if(old_sensor==0x00) { write_duty_pwm0(duty_cycle=0); write_duty_pwm1(duty_cycle=300); } SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page 78 GVHD:Th.s Hồng Trung Hiếu Thiết kế, thi cơng điều khiển Robot else if(old_sensor==0x03) { write_duty_pwm0(duty_cycle=0); write_duty_pwm1(duty_cycle=300); } else { write_duty_pwm0(duty_cycle=0); write_duty_pwm1(duty_cycle=300); } old_sensor=sensor; break; } case 0x10: { if(old_sensor==0x18) { write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=100); } else if(old_sensor==0x30) { write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=150); } else { write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=50); } old_sensor=sensor; SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page 79 GVHD:Th.s Hoàng Trung Hiếu Thiết kế, thi công điều khiển Robot break; } case 0x02: { if(old_sensor==0x03) { write_duty_pwm0(duty_cycle=150); write_duty_pwm1(duty_cycle=300); } else if(old_sensor==0x06) { write_duty_pwm0(duty_cycle=50); write_duty_pwm1(duty_cycle=300); } else { write_duty_pwm0(duty_cycle=100); write_duty_pwm1(duty_cycle=300); } old_sensor=sensor; break; } case 0x00: { if(old_sensor==0x08) { write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=300); } else if(old_sensor==0x04) { SVTH: Đỗ Ngọc Dũng –710015D -07DD3N Page 80 GVHD:Th.s Hồng Trung Hiếu Thiết kế, thi cơng điều khiển Robot write_duty_pwm0(duty_cycle=300); write_duty_pwm1(duty_cycle=300); } else if(old_sensor==0x01) { write_duty_pwm0(duty_cycle=0); output_low(pin_b0); delay_ms(50); xung=100; while(sensor==0x00) { sensor=input_a(); sensor=sensor&0x3f; write_duty_pwm1(duty_cycle=xung); if(xung