TRƯỜNG ĐẠI HỌC LẠC HỒNG KHOA CƠ ĐIỆN – ĐIỆN TỬ BÁO CÁO NGHIÊN CỨU KHOA HỌC ĐỀ TÀI THIẾT KẾ VÀ CHẾ TẠO MÔ HÌNH XE HAI BÁNH TỰ CÂN BẰNG LÊ XUÂN HƯNG BIÊN HÒA, THÁNG 12/2013 TRƯỜNG ĐẠI HỌC LẠC HỒNG K[.]
TRƯỜNG ĐẠI HỌC LẠC HỒNG KHOA CƠ ĐIỆN – ĐIỆN TỬ - - BÁO CÁO NGHIÊN CỨU KHOA HỌC ĐỀ TÀI: THIẾT KẾ VÀ CHẾ TẠO MÔ HÌNH XE HAI BÁNH TỰ CÂN BẰNG LÊ XUÂN HƯNG BIÊN HÒA, THÁNG 12/2013 TRƯỜNG ĐẠI HỌC LẠC HỒNG KHOA CƠ ĐIỆN – ĐIỆN TỬ - - BÁO CÁO NGHIÊN CỨU KHOA HỌC ĐỀ TÀI: THIẾT KẾ VÀ CHẾ TẠO MƠ HÌNH XE HAI BÁNH TỰ CÂN BẰNG Sinh viên thực hiện: LÊ XUÂN HƯNG Giáo viên hướng dẫn: ThS.LÊ HỒNG ANH BIÊN HỊA, THÁNG 12/2013 i LỜI CẢM ƠN Trong trình thực đề tài em nhận nhiều hỗ trợ giúp đỡ quý thầy cô, bạn sinh viên khoa Cơ điện-Điện tử Trước hết, em xin chân thành cảm ơn thầy ThS Lê Hoàng Anh giảng viên trực tiếp hướng dẫn em thực đề tài Thầy hướng dẫn, nhắc nhở, hỗ trợ trang thiết bị giúp em thực thành công đề tài Tiếp theo, em xin gửi lời cảm ơn thầy ThS Đỗ Bình Nguyên thầy khoa Cơ điện-Điện tử trường Đại học Lạc Hồng cho em lời khuyên, kinh nghiệm quý báu tạo điều kiện thuận lợi để em hoàn thành đề tài Ngoài ra, em xin cảm ơn anh Trần Nguyên Sang, bạn Trà Ti Na Nguyễn Tấn Hậu hỗ trợ em nhiều suốt thời gian qua Cuối cùng, em xin gửi lời cảm ơn chân thành đến gia đình em, cha mẹ động viên cổ vũ cho em suốt năm tháng học tập ii MỤC LỤC Lời cảm ơn .i Mục lục ii Bảng danh mục từ viết tắt v Danh mục bảng biểu vii Danh hình ảnh viii Chương 1: TỔNG QUAN 1.1 Mục tiêu đề tài 1.2 Giới hạn đề tài 1.3 Phương pháp tiếp cận đề tài 1.4 Tình hình nghiên cứu ngồi nước 1.4.1 Tình hình nghiên cứu giới .2 1.4.1.1 Xe Segway 1.4.1.2 Balancing scooter .3 1.4.2.Tình hình nghiên cứu nước .4 1.5 Nội dung đề tài Chương 2: KẾT CẤU CƠ KHÍ VÀ THIẾT KẾ HỆ THỐNG ĐIỆN .10 2.1 Mơ hình khí 10 2.1.1 Khung xe 10 2.1.2 Cần điều khiển 11 2.1.3 Hộp giảm tốc 12 2.1.4 Bánh xe 14 iii 2.2 Hệ thống mạch điện xe 15 2.2.1 Sơ đồ khối hệ thống 15 2.2.2 Mạch điều khiển động 15 2.2.3 Cảm biến IMU 16 2.2.4 Mạch điều khiển trung tâm 17 2.2.5 Động Cơ 18 2.2.6 Nguồn điện .20 2.3 Mơ hình xe thực tế 21 2.4 Các phương thức giao tiếp 21 2.4.1 Giao thức 21 2.4.2 ADC 22 2.4.3 UART .23 2.4.3 PWM 24 Chương 3: GIẢI THUẬT ĐIỀU KHIỂN CÂN BẰNG VÀ DI CHUYỂN 26 3.1 Nguyên lý điều khiển cân di chuyển 26 3.2 Bộ lọc Kalman 28 3.3 Giải thuật PID 30 3.5 Giải thuật cân điều khiển di chuyển 32 Chương KẾT QUẢ THỰC NGHIỆM 35 4.1 Mơ hình khí 35 4.2 Độ xác cảm biến 35 4.3 Kết thực nghiệm mơ hình xe thực tế 36 Chương KẾT LUẬN 38 iv 5.1 Kết luận 38 5.2 Hướng phát triển đề tài 38 Tài liệu tham khảo 39 v BẢNG DANH MỤC CÁC TỪ VIẾT TẮT A - Ampe ADC - Analog to Digital Converter CPU - Central Processing Unit DAC - Digital to Analog Converter DC - Direct Current EEPROM - Electrically Erasable Programmable Read Only Memory IMU - Inertial Measurement Unit I2 C - Inter Intergrated Circuit IC - International Circuit Ibs - Paund I/O - Input / Output LED - Light Emitting Diode LCD - Liquid-crystal display MENS - Microelectromechanical Systems MOSFET - Metal–oxide–semiconductor field-effect transistor MSB - Most Significant Bit OTG - On The Go PC - Personal Computer PID - Proportional Integral Derivative vi PWM - Pulse Width Modulation USB - Universal Serial Bus UART - Universal Asynchronous Receiver – Transmitter Segway PT - Segway Personal Transporter SCL - Simply Connect Last SDA - Serial Data Line TTL - Transistor-transistor logic V - Volt W - Watt vii DANH MỤC BẢNG BIỂU Bảng 2.1 Thông số kỹ thuật bo Arduino Due AT91SAM3X8E .18 Bảng 3.1 Phương pháp Ziegler - Nichols Bảng 4.1 Bảng so sánh kết đạt với xe Segway hãng 37 viii DANH MỤCHÌNH ẢNH Hình 1.1 Xe Segway Hình 1.2 Tác giả Trevor Blackwell xe scooter Hình 1.3 Robot hai bánh cân Nguyễn Gia Minh Thảo .5 Hình 1.4 Robot hai bánh cân Trà Ti Na Nuyễn Tấn Hậu Hình 1.5 Xe hai bánh tự cân Mai Tuấn Đạt Hình 1.6 Xe hai bánh tự cân Phan Ngọc Anh Tùng Hình 2.1 Sơ đồ khối hệ thống khí xe .10 Hình 2.2 Khung xe 11 Hình 2.3 Cần điều khiển 11 Hình 2.4 Khớp xoay gối đỡ cần điều khiển 12 Hình 2.5 Biến trở dùng để điều khiển chuyển hướng xe 12 Hình 2.6 Hộp giảm tốc 13 Hình 2.7 Bánh xe .14 Hình 2.8 Sơ đồ khối mơ hình xe 15 Hình 2.9 Board điều khiển động 15 Hình 2.10 Sơ đồ kết nôi MPU 6050 với board Arduino 16 Hình 2.11 Board Arduino Due 17 Hình 2.12 Động DC .20 Hình 2.13 Bình ắcquy .20 Hình 2.14 Mơ hình xe thực tế 21 Hình 2.15 Mạng I2C với nhiều thiết bị điện trở kéo lên cho SDA, SCL 22 Môi trường phát triển (IDE) Môi trường phát triển Arduino phát triển dựa ngôn ngữ lập trình Processing dự án Wiring Được thiết kế cho người khơng biết nhiều lập trình, bao gồm trình soạn thảo có đánh dấu cú pháp, tự động đóng mở cặp ngoặc nhọn, tự động thụt cách đầu dịng Kèm theo khả biên dịch nạp chương trình vào chip vi điều khiển Một chương trình viết Arduino gọi “sketch” Như nói trên, Arduino dự án mã nguồn mở, download thứ miễn phí, bao gồm IDE từ trang chủ Arduino IDE không cần phải cài đặt, cần download tập tin nén từ trang chủ, giải nén chạy tập tin arduino.exe Khi đó, hình xuất cửa sổ sau: Hình phụ lục 1.3 Giao diện chương trình Arduino Các nút công cụ: Verify: Kiểm tra mã nguồn có khớp với chương trình bo Arduino hay không Upload: Biên dịch tải mã nguồn lên Arduino New: Tạo tập tin mã nguồn Open: Mở tập tin mã nguồn Save: Lưu tập tin mã nguồn Serial Monitor: Màn hình giao tiếp nối tiếp, dùng để xem liệu gửi từ Arduino Ở không gian soạn thảo mã nguồn Ở cửa số Output, nơi hiển thị thông báo biên dịch upload chương trình lên bo Arduino Để sử dụng Arduino, cắm bo Arduino vào cáp USB Cài driver cho bo Arduino (nếu chưa cài) Mở chương trình lập trình Arduino lên thực việc lập trình Các ứng dụng bật bo mạch Arduino Arduino chọn làm não xử lý nhiều thiết bị từ đơn giản đến phức tạp Trong số có vài ứng dụng thực chứng tỏ khả vượt trội Arduino chúng có khả thực nhiều nhiệm vụ phức tạp Sau danh sách số ứng dụng bật Arduino Máy in 3D Một cách mạng khác âm thầm định hình nhờ vào Arduino, phát triển máy in 3D nguồn mở Reprap Máy in 3D công cụ giúp tạo vật thể thực trực tiếp từ file CAD 3D Công nghệ hứa hẹn nhiều ứng dụng thú vị có cách mạng hóa việc sản xuất cá nhân Hình phụ lục 1.4 Máy in 3D Makerbot điều khiển Arduino Mega2560 Robot Do kích thước nhỏ gọn khả xử lý mạnh mẽ, Arduino chọn làm xử lý trung tâm nhiều loại robot Hình phụ lục 1.5 Robot hai bánh tự cân Mơ hình bay cánh Arduino chip có tốc độ xử lý nhanh, thích hợp với mơ hình địi hỏi tốc độ xử lý cao mơ hình bay, robot hai bánh tự cân … Hình phụ lục 1.6 Mơ hình bay cánh Trên vài ví dụ minh họa cho khả ứng dụng Arduino Khi tìm kiếm Google, tìm thấy vơ số ứng dụng có sử dụng bo mạch Arduino PHỤ LỤC SƠ ĐỒ NGHUYÊN LÝ MẠCH ĐIỀU KHIỂN ĐÔNG CƠ 12V 12V D4 4148 D3 24V R16 12V 4148 C5 104 IN VS HO LO C7 R7 100 IR2184 Q3 IRF540 Q4 IRF540 R8 100 104 R10 100 IN VS PWM2 SD HO LO J9 J7 J6 R11 100 CON1 D1 J4 VB C6 104 D12 12V IR2184 VCC Q2 IRF540 SD COM Q1 IRF540 COM VCC PWM1 VB 1K U16 J5 CON2 U15 CON1 CON1 R1 5V R15 330 5V 5V 330 10K 10K PGD PGC DIR_IN PWM_IN 11 12 5V D5 D6 LED ICSP 15 14 LED PGC/EMUC/U1RX/SDI1/SDA/C1RX/RF2 EMUD1/SOSCI/T2CK/U1ATX/CN1/RC13 PGD/EMUD/U1TX/SDO1/SCL/C1TX/RF3 EMUC1/SOSCO/T1CK/U1ARX/CN0/RC14 EMUC2/OC1/IC1/INT1/RD0 EMUD2/OC2/IC2/INT2/RD1 PC817 U18 PWM2 PGC PGD 18 17 Y1 7.5 MHz C1 22p CON4 C2 22p 5V 12V 5V R3 10K 78L05 + C3 1000uF C8 104 C4 104 GND VIN TIP122 R18 1K VOUT ISO2 24V Q5 U17 + C9 1000uF 6N137 Hình phụ lục 2.1 Sơ đồ nguyên lý mạch điều khiển động J3 D14 12V R5 330 PWM1 26 25 24 23 22 21 16 dsPIC30F4012 R4 330 27 19 R2 10K J10 PWM1L/RE0 PWM1H/RE1 PWM2L/RE2 PWM2H/RE3 PWM3L/RE4 PWM3H/RE5 FLTA/INT0/SCK1/OCFA/RE8 OSC1/CLKI R14 MCLR ENC_Z ENC_A ENC_B EMUD3/AN0/VREF+/CN2/RB0 EMUC3/AN1/VREF-/CN3/RB1 AN2/SS1/CN4/RB2 AN3/INDX/CN5/RB3 AN4/QEA/IC7/CN6/RB4 AN5/QEB/IC8/CN7/RB5 J2 R6 R9 5V OSC2/CLKO/RC15 CON4 MCLR AVSS VSS VSS VDD VDD AVDD U14 MCLR 10K 13 20 28 4148 ENC_A ENC_B 10 CON2 PHỤ LỤC SƠ ĐỒ NGUYÊN LÝ MẠCH CẢM BIẾN IMU GY-86 U3 Q3 3.3V VCC _3.3V VDD_IO S1 VDD VCC _5V C6 I O R4 C7 GND 1u SCL SDA D4 0.1u LED 0.1u 1K C126 10u C23 13 SETP SETC ON DRDY C1 LD0 3.3V GND GND XCL 16 XDA 12 C24 10 224 11 C25 15 475 HMC5883L DRDY 3.3V VCC_3.3V VCC_5V R13 SCL VDD PS GND R12 4.7K H_SCL CSB CSB SDO VCC_5V R14 4.7K H_SDA SDA/SDI SCLK IC2 SDA SCL MS5611 N_M OS U6 5V J1 CON 0.1u R15 4.7K SDA 4.7K VCC_3.3V C23 IC3 3.3V H_SCL H_SDA FSYNC C21 0.001uF XDA XCL 3.3V 10 C22 0.1uF FSYNC 11 12 INTA INTA DRDY CLKIN AUX_DA AUX_CL VLOGIC AD0 REGOUT SDA SCL CLKOUT RESV CPOUT RESV FSY NC IN GND VDD R103 1K 24 23 SDA SCL 22 21 20 19 C27 2200pF 18 13 3.3V MPU 6050 C25 0.1uF Hình phụ lục 3.1 Sơ đồ nguyên lý mạch cảm biến IMU GY-86 PHỤ LỤC SƠ ĐỒ NGUYÊN LÝ MẠCH ARDUINO DUE +3V3 RN3D 1K ON + - IC1A LMV358IDGKR C20 L RN3C 1K Y ELLOW MASTER-RESET 10n GND +3V3 GND GREEN PWM13 +3V3 CONNECT TXL TX Y ELLOW RN3B 1K RX Y ELLOW RN3A 1K TS42 RESET CONNECT RXL GND 100K RN2D 100K RN2A 100K RN1A 100K RN1B 100K RN1D 100K RN1C 100K RN2B JTAG 100K RN2C +3V3 JTAG_TMS JTAG_TCK JTAG_TDO JTAG_TDI JTAG_RESET 10 R1 0R MASTER-RESET GND COMMUNICATION +5V SCL0-3 +3V3 SDA0-3 RXD2 TXD2 RXD1 MASTER-RESET TXD1 RXD0 TXD0 MOSI 3X2M PWMH SCL1 10 SDA1 AREF GND PWM13 PWM12 PWM11 SS0/PWM10 PWM9 PWM8 10X1F-H8.5 PWML PWM7 PWM6 PWM5 SS1/PWM4 PWM3 PWM2 TX RX 8X1F-H8.5 J4 GND 36 CANTX1/IO 34 32 PIN51 30 PIN49 28 PIN47 26 PIN45 24 PIN43 22 PIN41 20 PIN39 18 PIN37 16 PIN35 14 PIN33 12 PIN31 10 PIN29 PIN27 PIN25 PIN23 35 33 31 29 27 25 23 21 19 17 15 13 11 18X2F-H8.5 XIO AD14(RXD3) PIN50 PIN48 PIN46 PIN44 PIN42 PIN40 PIN38 PIN36 PIN34 PIN32 PIN30 PIN28 PIN26 PIN24 PIN22 VIN 8X1F-H8.5 8X1F-H8.5 +3V3 GND ADCL AD7 AD6 AD5 AD4 AD3 AD2 AD1 AD0 ADCH CANTX0 CANRX0 DAC1 DAC0(CANRX1)5 AD11(TXD3) AD10 AD9 AD8 8X1F-H8.5 +5V POWER SCL0-3 SCL0-3 8X1F-H8.5 100K RN5B GND SPI MISO SPCK MASTER-RESET 100K RN5A +5V SCL0-3 SCL0-3 C26 10n GND XUSB +5V USBVCC MF-MSMF050-2 500mA +3V3 POWER USB1 XUSB OUTGID Z1 S1 S2 S3 S4 C27 C35 10u 10u MASTER-RESET CG0603MLC-05E R2 Z2 10K GND VIN 8X1F-H8.5 GND +3V3 GND Z5 MICRO_USB_AB1 OUTGND 100K RN5B ADCH CANTX0 CANRX0 DAC1 DAC0(CANRX1)5 AD11(TXD3) AD10 AD9 AD8 100K RN5A P$1 P$2 P$3 P$4 P$5 P$6 P$7 P$8 P$9 P$10 P$11 G1 G2 G3 G4 G5 G6 G7 G8 G9 G10 G11 CG0603MLC-05E SH1 SH2 SH3 SH4 F1 DD+ CG0603MLC-05E VUSB DD+ ID UGND SCL0-3 SCL0-3 SCL0-3 SCL0-3 8X1F-H8.5 +5V D3 CD1206-S01575 U5 FB 10n L4 10u SRR0604-100ML LM268 GND JTAG_TCK JTAG_TMS MASTER-RESET PC2 + STRIP 4x1V M 47u 10K R11 GND DEBUG SS1P3L EN SW C42 10u EN D4 10K C41 + VIN BOOST 52K3 R12 R10 GND VIN GND GND GND GND RN4B 100K 100K RN4C VIN GND +5V +3V3 R3 Q2 + - IC1B +3V3 LMV358IDGKR C31 100K RN4D +5V 100n IC4 NCP1117ST33T3G -3V3 + FDN340P C29 10u T2 UOTGVBOF T1 USBVCC BC847BSMD 100K RN4A 1K FDN340P USBVCC GND GND GND L1 MH2029-300Y VDDANA +3V3 L2 MH2029-300Y VDDOUTMI + C1 10u + C18 C4 C5 C6 C7 C8 C9 C2 100n 100n 100n 100n 100n 100n 100n 10u + C10 C3 100n 10u VDDOUT VDDOUT GND C11 + C12 C13 C13 C14 C14 C15 C15 C16 C16 C17 C17 10u 100n 100n 100n 100n 100n 100n 100n 100n 100n 100n 100n GND J2 JP5 8PB7 8PB6 3X2M R13 GND 10K D2 U10 RESET2 24 C40 22p GND C39 22p GND GND R9 Y 12MHz KX-7 20pF 1M_NM (PCINT7/OC0A/OC1C)PB7 RESET(PC1/DW) (PCINT6)PB6 (PCINT5)PB5 (T1/PCINT4)PB4 XTAL2(PC0) (PD0/MISO/PCINT3)PB3 (PDI/MOSI/PCINT2)PB2 XTAL1 (SCLK/PCINT1)PB1 (SS/PCINT0)PB0 +5V 32 C36 C38 100n 1u VUCAP USBVCCU2 RDRD+ GND GND 27 31 30 29 28 PAD GND GND C20 CAP NP (INT4/ICP1/CLK0)PC7 (OC1A/PCINT8)PC6 (PCINT9/OC1B)PC5 (PCINT10)PC4 (AIN2/PCINT11)PC2 AVCC VCC GND 21 20 19 18 17 MISO2 16 MOSI2 15 SCK2 14 R23 10K MASTER_CMD 22 23 25 26 RESET_CMD ERASE_CMD R21 1K N$1 USBVCC USB boot En GND +5V (CTS/HWB/AIN6/TO/INT7)PD7 UCAP (RTS/AIN5/INT6)PD6 UVCC (XCK/AIN4/PCINT12)PD5 D(INT5/AIN3)PD4 D+ (TXD1/INT3)PD3 UGND (RXD1/AIN1/INT2)PD2 (AIN0/INT1)PD1 33 OC0B/INT0)PD0 13 12 11 10 TX1 Y ELLOW TXL-U2 RXL-U2 16U2-TX TX R14 RX1 Y ELLOW GND R15 1K 1K ATMEGA16 GND IC10A 16U2-TX 2 MOSI2 CD1206-S01575 ICSP1 MISO2 SCK2 RESET2 8PB5 8PB4 +5V RX MF-MSMF050-2 500mA USBVCCU2 74LVC1G125DCK U11 USB B 22R RDR20 BD+ Z3 Z4 GND CG0603MLC-05E SH1 SH2 SH3 SH4 SH5 SH6 SH7 SH8 SH9 SH10 SH11 SH12 SH13 SH14 TX R19 BD- P1 P2 P3 P4 P5 CG0603MLC-05E S1 S2 S3 S4 S5 S6 P$1 P$2 P$3 P$4 P$5 P$6 P$7 P$8 F2 VUSB DD+ ID GND 22R RD+ TX +3V3 JR1 0R L3 MH2029-300Y NM AREF AVREF BR1 C19 C34 100n C21 22p 100n GND GND VDDPLL DR5 39R 1% R6 39R 1% D+ 38 43 37 42 40 100n 10u GND +3V3 74 33 54 44 XOUT32 XIN32 PD10 PD9 PD8 PD7 PD6 PD5 PD4 PD3 PD2 PD1 PD0 DHSDM DFSDM DHSDP DFSDP VBG R7 U1 C5 22p GND PA0 PA1 PA2 PA3 PA4 PA5 PA6 PA7 PA8 PA9 PA10 PA11 PA12 PA13 PA14 PA15 PA16 PA17 PA18 PA19 PA20 PA21 PA22 PA23 PA24 PA25 PA26 PA27 PA28 PA29 SAM3X4E PC30 PC29 PC28 PC27 PC26 PC25 PC24 PC23 PC22 PC21 PC20 PC19 PC18 PC17 PC16 PC15 PC14 PC13 PC12 PC11 PC10 PC9 PC8 PC7 PC6 PC5 PC4 PC3 PC2 PC1 PC0 ETX_CLK 113 ETX_EN 114 ETXD0 115 ETXD1 118 ERX_DV 119 ERXD0 120 ERXD1 121 ERX_ER 122 EMDC 123 EMDI0 127 UOTGVBOF 128 UOTGID 129 SDA0-3 86 SCL0-3 87 CANTX1/IO 140 DAC0(CANRX1) 76 DAC1 77 AD8 88 AD9 89 AD10 90 AD11(TXD3) 91 AD14(RXD3) 92 141 SS3 142 143 PWM2 144 PIN22 PWM13 68 JTAG_TCK 28 JTAG_TDI 29 JTAG_TDO 30 JTAG_TMS 31 (ETH) (SD) CANTX0 23 CANRX0 24 AD7 85 AD6 84 AD5 83 EEXTINT 25 AD4 82 PIN31 26 RX 27 TX RXD2 TXD2 RXD1 TXD1 PIN23 PIN24 AD0 78 SDA1 SCL1 70 PIN42 71 PIN43 72 TXL 107 AD3 81 AD2 80 AD1 79 MISO 108 MOSI 109 SPCK 110 SS0/PWM10 111 SS1/PWM4 112 PB0 PB1 PB2 PB3 PB4 PB5 PB6 PB7 PB8 PB9 PB10 PB11 PB12 PB13 PB14 PB15 PB16 PB17 PB18 PB19 PB20 PB21 PB22 PB23 PB24 PB25 PB26 PB27 PB28 PB29 PB30 PB31 6K8 1% GND1 GND2 GND3 GND4 12 58 106 126 GND GNDANA GNDPLL GNDBU GNDUTMI 10 45 61 104 124 VDDCORE1 VDDCORE2 VDDCORE3 VDDCORE4 VDDCORE5 XOUT XIN 56 34 73 52 41 57 11 62 105 125 VDDPLL 32 22 21 20 19 18 17 16 15 14 13 PIN32 PIN30 PWM12 PWM11 PIN29 RXD0 TXD0 PIN28 PIN27 PIN26 PIN25 103 102 139 138 137 136 135 134 133 132 131 101 100 99 98 97 96 95 94 93 117 67 66 65 64 63 116 60 59 55 130 RXL SS0/PWM10 PWM3 SS1/PWM4 PWM5 PWM6 PWM7 PWM8 PWM9 PIN44 PIN45 PIN46 PIN47 PIN48 PIN49 PIN50 PIN51 PIN41 PIN40 PIN39 PIN38 PIN37 PIN36 PIN35 PIN34 PIN33 ERASE ERASE_CMD 49 48 TST C33 + VDDANA VDDOUT VDDPLL 35 36 C32 VDDANA VDDBU VDDOUMI 51 XOUT XIN 53 50 39 0R_NM VBUS R8 JTAGSEL C24 22pF_NM 46 Y2 32.768Khz FWUP SHDN GND N.C XOUT32 VDDOUTMI VDDIN VDDIO1 VDDIO2 VDDIO3 VDDIO4 GND C23 22pF_NM GND XUSB ADVREF 100K MASTER-RESET N.C - internal pullup 75 AVREF Y 12MHz KX-7 20pF NRSTB NRST C22 22p L5 MH2029-300Y R4 47 69 GND GND +3V3 GND T3 +3V3 FDN340P ERASE_CMD EEXTINTA ERX_DVA EMDIOA EMDCA L6 MH2029-300Y 13 ERX_ERA 11 ERXD1A ERXD0A ETXD1A ETXD0A ETX_ENA ETX_CLKA GND IC10P MACB VIN ETH D1 PC1 M7 47u GND 14 12 10 2 1 X2 POWERSUPPLY _DC21MMX 3 22p +3V3 RN4B 100K GND C39 GND GND Hình phụ lục 4.1 Sơ đồ nguyên lý mạch Arduino Due RESETA PHỤ LỤC CODE CHƯƠNG TRÌNH #include "I2Cdev.h" #include "MPU6050.h" #include "Kalman.h" #include "pwm01.h" #include #include #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h" #endif MPU6050 accelgyro; Kalman kalmanY; int16_t accX, accY, accZ; int16_t tempRaw; int16_t gyroX, gyroY, gyroZ; uint32_t timer; double accXangle, accYangle; double temp; double gyroXangle, gyroYangle; double Setpoint, Input, Output; double Kp,Ki,Kd; double kalAngleX, kalAngleY; long frequency = 15000; uint16_t PWM_L,PWM_R,PWM_T; char PIN_PWM_L = 8; char PIN_DIR_L = 23; char PIN_PWM_R = 9; char PIN_DIR_R = 25; char PIN_BUTTON = 27; #define LED_PIN 13 double Angle; double SetAngle = 88; bool dir = 0; bool play = 0; char counter; bool blinkState = false; double CounterEncoder= 0; double Value; PID myPID(&Input, &Output, &Setpoint,0,0,0, DIRECT); void timerIsr()//ngat dieu khien PID { Angle = kalAngleY; if(Angle (SetAngle + 20)) Angle= SetAngle + 20; SetAngle=88-Value; Input = SetAngle-Angle; myPID.Compute();//Thuat toan PID (Output >= 0)? dir=0:dir=1; digitalWrite(PIN_DIR_L,dir); digitalWrite(PIN_DIR_R,dir); PWM_T=4095-abs(Output); PWM_L= PWM_T; PWM_R= PWM_T*0.97;//Dap ung sai lech cua DC pwm_write_duty( PIN_PWM_R, PWM_R ); pwm_write_duty( PIN_PWM_L, PWM_L ); } void setup() { Serial.begin(9600); #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif accelgyro.initialize(); accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG; accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG; kalmanY.setAngle(accYangle); gyroYangle = accYangle; timer = micros(); Kp=90; Ki=800; Kd=0.03; Setpoint = 0; myPID.SetOutputLimits(-2000,2000); myPID.SetTunings(Kp,Ki,Kd); myPID.SetMode(AUTOMATIC); pwm_set_resolution(12); pwm_setup( 6, frequency, 1); pwm_setup( 7, frequency, 2); pwm_setup( 8, frequency, 2); pwm_setup( 9, frequency, 2); pwm_write_duty( PIN_PWM_L, 4095); pwm_write_duty( PIN_PWM_R, 4095); pinMode(LED_PIN, OUTPUT); pinMode(PIN_DIR_L, OUTPUT); pinMode(PIN_DIR_R, OUTPUT); pinMode(PIN_BUTTON, INPUT); pinMode(39, INPUT); pinMode(41, INPUT); attachInterrupt(39, Encoder, RISING); } void loop() { accelgyro.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ); accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG; double gyroYrate = -((double)gyroY/131.0); gyroYangle += gyroYrate*((double)(micros()-timer)/1000000); kalAngleY = kalmanY.getAngle(accYangle, timer)/1000000); timer = micros(); temp = ((double)tempRaw + 12412.0) / 340.0; if(digitalRead(PIN_BUTTON)==1) { play=1; gyroYrate, (double)(micros()- Timer3.attachInterrupt(timerIsr); Timer3.start(1000); } if(play==0) Timer.stop(); counter++; if(counter==20) { counter=0; Value=(CounterEncoder/3300); Value = constrain(Value,-5,5); } Serial.print(88);Serial.print(','); Serial.print(kalAngleY);Serial.println(','); } void Encoder() { if(((digitalRead(39)==0)&&(digitalRead(41)==1)) || ((digitalRead(39)==1) && (digitalRead(41)==0))) CounterEncoder ; else if(((digitalRead(39)==0)&&(digitalRead(41)==0)) || ((digitalRead(39)==1) && (digitalRead(41)==1))) CounterEncoder++; }