Điều khiển xe hai bánh cân bằng và leo dốc

ĐẠI HỌC QUỐC GIA TP HỒ CHÍ MINH TRƢỜNG ĐẠI HỌC BÁCH KHOA LÂM VĂN LINH ĐIỀU KHIỂN XE HAI BÁNH CÂN BẰNG VÀ LEO DỐC Chuyên ngành: TỰ ĐỘNG HĨA LUẬN VĂN THẠC SĨ TP HỒ CHÍ MINH, THÁNG 07 NĂM 2012 CƠNG TRÌNH ĐƢỢC HỒN THÀNH TẠI TRƢỜNG ĐẠI HỌC BÁCH KHOA – ĐHQG – HCM Cán hƣớng dẫn khoa học : TS NGUYỄN VĨNH HẢO (Ghi rõ họ, tên, học hàm, học vị chữ ký) Cán chấm nhận xét : (Ghi rõ họ, tên, học hàm, học vị chữ ký) Cán chấm nhận xét : (Ghi rõ họ, tên, học hàm, học vị chữ ký) Luận văn thạc sĩ đƣợc bảo vệ Trƣờng Đại Học Bách Khoa, ĐHQG TP.HCM, ngày tháng năm Thành phần Hội đồng đánh giá luận văn thạc sĩ gồm: (Ghi rõ họ, tên, học hàm, học vị Hội đồng chấm bảo vệ luận văn thạc sĩ) ………………………………………………………… ………………………………………………………… ………………………………………………………… ………………………………………………………… ………………………………………………………… Xác nhận Chủ tịch Hội đồng đánh giá luận văn Trƣởng khoa quản lý chuyên ngành sau luận văn đƣợc sửa chữa (nếu có) CHỦ TỊCH HỘI ĐỒNG TRƢỞNG KHOA QLCN TRƢỜNG ĐẠI HỌC BÁCH KHOA CỘNG HOÀ XÃ HỘI CHỦ NGHIÃ VIỆT NAM KHOA ĐIỆN - ĐIỆN TỬ Độc Lập - Tự Do - Hạnh Phúc -o0o Tp HCM, ngày tháng năm NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên: LÂM VĂN LINH Phái: Nam Ngày, tháng, năm sinh: 22/05/1979 Nơi sinh: Sóc Trăng Chun ngành: TỰ ĐỘNG HĨA MSHV: 10150041 1- TÊN ĐỀ TÀI: ĐIỀU KHIỂN XE HAI BÁNH CÂN BẰNG VÀ LEO DỐC 2- NHIỆM VỤ LUẬN VĂN: - Mơ hình hóa đối tƣợng xe hai bánh tự cân - Nghiên cứu, tìm hiểu phƣơng pháp điều khiển phi tuyến để thiết kế điều khiển cho đối tƣợng xe hai bánh tự cân - Thực mô điều khiển phi tuyến với đối tƣợng xe hai bánh cân MATLAB/Simulink - Thiết kế thi cơng hồn chỉnh mơ hình phần cứng xe hai bánh cân - Lập trình theo phƣơng pháp điều khiển phi tuyến tìm hiểu cho mơ hình xe hai bánh cân thực nghiệm Điều khiển xe lên dốc nghiêng nhỏ - So sánh chất lƣợng điều khiển điều khiển phi tuyến với điều khiển PID kết mô 3- NGÀY GIAO NHIỆM VỤ: 20/08/2011 4- NGÀY HOÀN THÀNH NHIỆM VỤ: 30/06/2012 5- HỌ VÀ TÊN CÁN BỘ HƢỚNG DẪN: TS NGUYỄN VĨNH HẢO Nội dung đề cƣơng Luận văn đƣợc hội đồng chuyên ngành thông qua C.BỘ HƢỚNG DẪN (Họ tên chữ ký) CHỦ NHIỆM BỘ MÔN (Họ tên chữ ký) KHOA QL CH.NGÀNH (Họ tên chữ ký) LỜI CẢM ƠN Trƣớc hết xin cảm ơn ba mẹ, gia đình ln ủng hộ nguồn động viên lớn, chỗ dựa vững Kính gửi đến thầy TS NGUYỄN VĨNH HẢO lời cảm ơn chân thành sâu sắc, cảm ơn thầy tận tình hƣớng dẫn suốt trình thực luận văn Thạc sĩ Xin cảm ơn quý thầy cô môn Điều Khiển Tự Động khoa Điện Điện Tử tận tình giảng dạy, trang bị cho chúng em kiến thức bổ ích khoảng thời gian học cao học Tôi xin cảm ơn tất ngƣời thân, bạn bè, đặc biệt nhóm học viên cao học khóa 2010 động viên, góp ý, giúp đỡ tơi nhiều suốt q trình học tập thực luận văn TP Hồ Chí Minh, tháng 07 năm 2012 Lâm Văn Linh TÓM TẮT NỘI DUNG LUẬN VĂN THẠC SĨ Nhiệm vụ đề tài thực thi ứng dụng phƣơng pháp điều khiển phi tuyến để thiết kế điều khiển phù hợp cho mơ hình xe hai bánh tự cân Phƣơng pháp điều khiển phi tuyến đƣợc tìm hiểu sử dụng luận văn điều khiển chiếu (Backstepping) điều khiển trƣợt (Sliding mode) Thực mô điều khiển chiếu, điều khiển trƣợt cho đối tƣợng xe hai bánh tự cân với phần mềm Matlab/Simulink Đồng thời so sánh kết mô điều khiển phi tuyến với điều khiển PID, nêu ƣu khuyết điểm điều khiển thiết kế đề tài Tác giả thiết kế thi công hồn chỉnh mơ hình xe hai bánh tự cân thực nghiệm Mơ hình xe gồm hai động DC-Servo có hộp số để truyền động, mơ-đun đo góc nghiêng thân xe gồm cảm biến gia tốc góc cảm biến vận tốc góc Các tín hiệu từ mơ-đun cảm biến góc nghiêng đƣợc đƣa vào lọc Kalman, tín hiệu ngõ của lọc Kalman tín hiệu góc nghiêng sau triệt nhiễu tiếp tục đƣợc đƣa vào mô-đun điều khiển trung tâm sử dụng vi điều khiển MC9S12XDT256 hãng FreeScale Vi điều khiển trung tâm đƣợc lập trình dựa phƣơng pháp điều khiển phi tuyến thực việc tính tốn xuất tín hiệu điều khiển qua mơ-đun cầu H có cách ly để điều khiển hai động DC-Servo quay để mơ hình xe hai bánh giữ thăng đƣợc Kết đạt đƣợc đáp ứng đƣợc yêu cầu đề tài, bao gồm: thiết kế hồn chỉnh điều góc nghiêng thân xe sử dụng lần lƣợt phƣơng pháp điều khiển chiếu, điều khiển trƣợt Ngoài ra, tác giả sử dụng điều khiển PD cho vị trí bánh xe Các điều khiển phi tuyến có kết mơ tốt, đáp ứng nhanh, vọt lố so với điều khiển PID Mơ hình xe thực nghiệm giữ thăng tốt, di chuyển tƣơng đối linh hoạt lên dốc nghiêng nhỏ [độ] Ngoài ra, ngƣời dùng điều khiển hoạt động mơ hình xe thực nghiệm điều khiển từ xa Abstract The main point of this thesis is to design and embed the non-linear control to manipulate the two-wheeled self-balancing robot The methods used in this paper are backstepping control and fuzzy sliding mode control in Matlab-Simulink presentation This paper also shows the comparison between these two methods and the classical PID control, and points out the advantages and disadvantages of them A completed model of two-wheeled self-balancing robot is given with two DC-Servo motors including gearbox, an IMU including Kalman filter design, a 16-bit MCU MC9S12XDT256 of FreeScale and a RF module to manipulate The main achievements of this thesis are to remain balancing state and to be able to climb over a 8-degree slope with these two non-linear methods Simulation and experimental results show that the methods have good performances in terms of quick response, good balance and stability Luận Văn Thạc Sĩ – Điều Khiển Xe Hai Bánh Cân Bằng Và Leo Dốc Mục lục CHƢƠNG 1: GIỚI THIỆU TỔNG QUAN VỀ ĐỀ TÀI 1.1 Đặt vấn đề: 1.2 Các cơng trình liên quan 1.2.1 Một số mơ hình robot hai bánh tự cân 1.2.1.1 Robot JOE - [2] 1.2.1.2 N-Bot, [19] 1.2.1.3 Xe hai bánh cân gom rác 1.2.1.4 Xe Segway PT , [20] 1.2.1.5 Xe di chuyển ngƣời hãng Toyota 10 1.2.2 Các báo cáo nghiên cứu khoa học liên quan 11 1.3 Phạm vi nghiên cứu 12 CHƢƠNG CƠ SỞ LÝ THUYẾT LIÊN QUAN 13 2.1 Nguyên lý hoạt động xe hai bánh cân bằng: 13 2.2 Lý thuyết phƣơng pháp điều khiển 14 2.2.1 Phƣơng pháp điều khiển phi tuyến 14 2.2.1.1 Điều khiển chiếu (Back-Stepping) 14 2.2.1.1.1 Cuốn chiếu qua khâu tích phân: 14 2.2.1.1.2 Trƣờng hợp tổng quát: 16 2.2.1.2 Phƣơng pháp điều khiển trƣợt(Sliding Mode): 17 2.2.1.2.1 Điều khiển bám (Tracking) 17 2.2.1.2.2 Ổn định hóa (regulation) 19 2.3 Lý thuyết lọc Kalman 20 2.3.1 Bản chất toán học lọc kalman 20 2.3.2 Bản chất thống kê lọc Kalman 22 2.3.3 Giải thuật lập trình lọc Kalman rời rạc 23 2.4 So sánh lọc với lọc Kalman 26 CHƢƠNG 3: THIẾT KẾ BỘ ĐIỀU KHIỂN CHO XE HAI BÁNH CÂN BẰNG 28 3.1 Mơ hình hóa xe hai bánh tự cân 28 3.2 Thiết kế điều khiển cho xe hai bánh cân 36 3.2.1 Bộ điều khiển đặt cực 36 3.2.2 Bộ điều khiển PID -PD 40 Trang Luận Văn Thạc Sĩ – Điều Khiển Xe Hai Bánh Cân Bằng Và Leo Dốc 3.2.3 Bộ điều khiển chiếu 41 3.2.4 Bộ điều khiển trƣợt (Sliding Mode) 45 3.3 So sánh kết mô hệ thống 48 3.4 Ƣu, khuyết điểm điều khiển thiết kế cho mơ hình hệ thống 53 CHƢƠNG 4: THI CƠNG MƠ HÌNH XE HAI BÁNH TỰ CÂN BẰNG 56 4.1 Thiết kế mơ hình khí 56 4.1.1 Sơ đồ cấu trúc khí robot 56 4.1.2 Động DC-Servo dùng để truyền động cho robot 58 4.2 Sơ đồ khối chức mạch phần cứng liên quan 61 4.2.1 Sơ đồ khối cảm biến đo góc nghiêng 61 4.2.2 Giới thiệu IC MC33932 chức mạch cầu H (phụ lục B) 64 4.2.3 Sơ đồ chức IC điều khiển trung tâm (phụ lục B) 65 4.2.4 Chức mạch giao tiếp với máy vi tính 67 4.2.5 Chức mạch điều khiển nút nhấn 67 4.2.6 Chức mạch hiển thị LCD 68 4.2.7 Mơ-đun thu phát tín hiệu qua sóng radio 69 4.3 Giải thuật chƣơng trình cho mơ hình xe hai bánh 70 4.3.1 Giải thuật chƣơng trình 70 4.3.2 Chi tiết hàm chƣơng trình 71 4.3.3 Khai báo quản lý ngắt 75 4.4 Cách thu nhận xử lý tín hiệu cảm biến 77 4.5 Mơ hình xe hai bánh thực nghiệm 78 CHƢƠNG 5: KẾT QUẢ THỰC NGHIỆM MƠ HÌNH 79 5.1 Xe giữ thăng không di chuyển 79 5.2 Xe giữ thăng di chuyển đến vị trí đặt trƣớc 81 5.3 Xe giữ thăng tác dụng ngoại lực 82 5.4 Xe giữ thăng di chuyển tiến – lùi 83 5.5 Xe giữ thăng lên dốc 84 CHƢƠNG 6: KẾT LUẬN VÀ HƢỚNG PHÁT TRIỂN 86 6.1 Các kết đạt đƣợc 86 6.2 Một số hạn chế 88 6.3 Hƣớng phát triển đề tài 89 Tài liệu tham khảo 90 Trang Luận Văn Thạc Sĩ – Điều Khiển Xe Hai Bánh Cân Bằng Và Leo Dốc PHẦN LÝ LỊCH TRÍCH NGANG: 92 Phụ lục A: Các mạch điện tử thi cơng mơ hình 93 Phụ lục B: Code chƣơng trình thu thập liệu PC 97 Phụ lục C: Code chƣơng trình điều khiển 106 Trang Luận Văn Thạc Sĩ – Điều Khiển Xe Hai Bánh Cân Bằng Và Leo Dốc CHƢƠNG 1: GIỚI THIỆU TỔNG QUAN VỀ ĐỀ TÀI 1.1 Đặt vấn đề: Trong ngành tự động hóa – điều khiển tự động nói chung điều khiển học nói riêng, mơ hình lắc ngƣợc đối tƣợng nghiên cứu điển hình đặc thù đặc tính động khơng ổn định mơ hình nên việc điều khiển đƣợc đối tƣợng thực tế đặt nhƣ thử thách Công nghệ robot đạt đƣợc nhiều thành tựu lớn xuất nhiều lĩnh vực nhƣ: robot dân dụng giúp việc gia đình, robot thực việc giải trí - quảng cáo, robot cơng nghiệp, robot tự hành thám hiểm lòng đất, robot thăm dò hành tinh khoa học vũ trụ Kết nghiên cứu mơ hình lắc ngƣợc bản, ví dụ nhƣ mơ hình xe-con lắc, lắc ngƣợc quay… ứng dụng kế thừa sang mơ hình tƣơng tự khác nhƣng có tính ứng dụng thực tiễn hơn, chẳng hạn nhƣ mơ hình tên lửa, mơ hình xe hai bánh tự cân bằng… Xuất phát từ ý tƣởng xe hai bánh tự cân Segway, phát minh tiếng Dean Kamen – kỹ sƣ ngƣời Mỹ vào năm 2001, mở hƣớng phát triển chế tạo robot mới, robot hai bánh tự cân Đây loại robot có hai bánh, tự giữ thăng bằng, di chuyển hoạt động dễ dàng khoảng không gian nơi mà linh hoạt, động, hiệu đƣợc đặt lên hàng đầu Với ƣu điểm đó, robot hai bánh tự cân nhận đƣợc nhiều quan tâm từ nhà nghiên cứu hãng sản xuất robot giới năm gần Robot hai bánh tự cân đƣợc xem nhƣ cầu nối kinh nghiệm mơ hình lắc ngƣợc với robot hai chân robot giống ngƣời Đây dạng robot có hai bánh đồng trục, khắc phục đƣợc nhƣợc điểm vốn có robot hai ba bánh kinh điển Các robot hai ba bánh kinh điển, theo có cấu tạo gồm bánh dẫn động bánh tự để đỡ trọng lƣợng robot Nếu trọng lƣợng đƣợc đặt nhiều vào bánh lái robot khơng ổn định dễ bị ngã, cịn đặt vào nhiều bánh hai bánh khả Trang Luận Văn Thạc Sĩ – Điều Khiển Xe Hai Bánh Cân Bằng Và Leo Dốc * * Again, note that C_1 is zero, so we not compute the term */ float E = R_angle + C_0 * PCt_0; /* + C_1 * PCt_1 = */ /* * Compute the Kalman filter gains From the Kalman paper: * * K = P C' inv(E) * * Dimensionally: * * K = P C' inv(E) * * Luckilly, E is , so the inverse of E is just 1/E */ float K_0 = PCt_0 / E; float K_1 = PCt_1 / E; /* * Update covariance matrix Again, from the Kalman filter paper: * * P=P-KCP * * Dimensionally: * * P -= K C P * * We first compute t = C P Note that: * * t[0,0] = C[0,0] * P[0,0] + C[0,1] * P[1,0] * * But, since C_1 is zero, we have: * * t[0,0] = C[0,0] * P[0,0] = PCt[0,0] * * This saves us a floating point multiply */ float t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] */ float t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1] = */ P[0][0] -= K_0 * t_0; P[0][1] -= K_0 * t_1; P[1][0] -= K_1 * t_0; P[1][1] -= K_1 * t_1; /* * Update our state estimate Again, from the Kalman paper: * * X += K * err * * And, dimensionally, Trang 110 Luận Văn Thạc Sĩ – Điều Khiển Xe Hai Bánh Cân Bằng Và Leo Dốc * * X = X + K * err * * err is a measurement of the difference in the measured state * and the estimate state In our case, it is just the difference * between the two accelerometer measured angle and our estimated * angle */ angle += K_0 * angle_err; q_bias += K_1 * angle_err; // Limitation -// // -90 5 - Make 8Khz Frequency square , Left-Aligned Note: Channel 0-1-4-5: use Clock A and Clcok SA Channel 2-3-6-7: use Clock B and Clock SB period = PWMPERX * 2^(PCKA(B)+1) * PWMSCLA(B) (@@@) (when: PWMCLK_PCLKX = 1: choose Clock SA(B) ) 200 * * 20 = 8000 ( Bus Clock is 40Mhz > period = Bus_clock / f_square = 40Mhz / 5Khz = 8000 ) Duty_cycle = PWMDTYx / PWMPERx */ void PWM_Init(void){ // - Common // PWMCTL = 0x00 ; // Separate 8-bit PWM Channels PWMPRCLK = 0x00 ; // PCKA = ; PCKB = :use for 8-Channels PWMSCLA = 20 ; // for (@@@) PWMSCLB = 20 ; // for (@@@) // -// // PWM0 -// PWMCLK_PCLK0 = ; // use Clock SA for PWM0-Channel PWMPOL_PPOL0 = ; // Polarity=1 > start with High-Signal in PWM0 PWMCAE_CAE0 = ; // PWM0 is Left-Aligned PWMPER0 = 200 ; // Periode0 = 200 > 5KHz of Square_Wave (@@@) PWME_PWME0 = ; // Enable PWM0 // // // PWM1 -// PWMCLK_PCLK1 = ; // use Clock SA for PWM0-Channel PWMPOL_PPOL1 = ; // Polarity=1 > start with High-Signal in PWM1 Trang 111 Luận Văn Thạc Sĩ – Điều Khiển Xe Hai Bánh Cân Bằng Và Leo Dốc PWMCAE_CAE1 = ; // PWM1 is Left-Aligned PWMPER1 = 200 ; // Periode0 = 200 > 5KHz of Square_Wave (@@@) PWME_PWME1 = ; // Enable PWM1 // // // PWM2 -// PWMCLK_PCLK2 = ; // use Clock SB for PWM2-Channel PWMPOL_PPOL2 = ; // Polarity=1 > start with High-Signal in PWM2 PWMCAE_CAE2 = ; // PWM2 is Left-Aligned PWMPER2 = 200 ; // Periode2 = 200 > 5KHz of Square_Wave (@@@) PWME_PWME2 = ; // Enable PWM2 // // // PWM3 -// PWMCLK_PCLK3 = ; // use Clock SB for PWM3-Channel PWMPOL_PPOL3 = ; // Polarity=1 > start with High-Signal in PWM3 PWMCAE_CAE3 = ; // PWM0 is Left-Aligned PWMPER3 = 200 ; // Periode3 = 200 > 5KHz of Square_Wave (@@@) PWME_PWME3 = ; // Enable PWM3 // // /* // PWM4 -// PWMCLK_PCLK4 = ; // use Clock SA for PWM4-Channel PWMPOL_PPOL4 = ; // Polarity=1 > start with High-Signal in PWM4 PWMCAE_CAE4 = ; // PWM4 is Left-Aligned PWMPER4 = 200 ; // Periode4 = 200 > 5KHz of Square_Wave (@@@) PWME_PWME4 = ; // Enable PWM4 // // // PWM5 -// PWMCLK_PCLK5 = ; // use Clock SA for PWM5-Channel PWMPOL_PPOL5 = ; // Polarity=1 > start with High-Signal in PWM5 PWMCAE_CAE5 = ; // PWM5 is Left-Aligned PWMPER5 = 200 ; // Periode5 = 200 > 5KHz of Square_Wave (@@@) PWME_PWME5 = ; // Enable PWM5 // // */ // Initial Value // PWMDTY0 = ; // 0% Hiduty is IN-1 of MC33932-A PWMDTY1 = ; // 0% Hiduty is IN-2 of MC33932-A PWMDTY2 = ; // 0% Hiduty is IN-3 of MC33932-B PWMDTY3 = ; // 0% Hiduty is IN-4 of MC33932-B /* PWMDTY4 = 200 ; // 100% Hiduty > Enable MC33932-A PWMDTY5 = 200 ; // 100% Hiduty > Enable MC33932-B */ // // } // -// // - TOC3_Init // Trang 112 Luận Văn Thạc Sĩ – Điều Khiển Xe Hai Bánh Cân Bằng Và Leo Dốc // TOC3 Init void TOC3_Init(void){ TSCR1 = 0x80; // Enable TCNT, 40MHz boot mode TSCR2 = 0x04; // divide by 16 TCNT prescale.+ Disable TOF Interrupt /* Bottom three bits of TSCR2 (PR2,PR1,PR0) determine TCNT period divide fast(24MHz) slow (4MHz) fastest: HCS12X(40Mhz) 000 42ns TOF 2.73ms 250ns TOF 16.384ms 25ns 001 84ns TOF 5.46ms 500ns TOF 32.768ms 010 167ns TOF 10.9ms 1us TOF 65.536ms 011 333ns TOF 21.8ms 2us TOF 131.072ms 200ns 100 16 667ns TOF 43.7ms 4us TOF 262.144ns 400ns Encoders of Motor TCTL3 |= 0b01010100 ; // PT5-6-7: Capture on Rising edge -> Encoders of Motor TFLG1 |= 0b11000011 ; // Clear Flags in PT0-1-2 and PT5-6-7 TIE |= 0b10000001 ; // Enabled interrupt in PT0 and PT7 } // -// //**** - Encoder of MOTOR ****// // - T1C0_Interrupt -// // For Encoder's A signal of Motor1 interrupt void TIC0hndlr(void){ // LEFT-MOTOR ( B_signal is faster than A_signal) char B_signal ; B_signal = PTT_PTT1 ; // get B_signal in PT1 if (B_signal == 0){ direction_flag_1 = ; count_pulse_1 -= ; } else if(B_signal == 1){ direction_flag_1 = ; count_pulse_1 += ; } // A is Raising-Pulse, B is Low > Reverse , pulse-// increase count-pulse // A is Raising-Pulse, B is High > Forward , pulse++ // decrease count-pulse Trang 113 Luận Văn Thạc Sĩ – Điều Khiển Xe Hai Bánh Cân Bằng Và Leo Dốc TFLG1 |= 0b00000001 ; // Clear Flag in PT0 } // // // Interrupt TIC7 - Encoder's A -// // interrupt when capture a raising pulse-A (Motor 2) interrupt 15 void TIC7hndlr(void){ // RIGHT-MOTOR ( A_signal is faster than B_signal) char B_signal ; B_signal = PTT_PTT6 ; // get B_signal in PT6 if (B_signal == 0){ direction_flag_2 = ; count_pulse_2 += ; } else if(B_signal == 1){ direction_flag_2 = ; count_pulse_2 -= ; } // A is Raising-Pulse, B is Low > Forward , pulse++ // increase count-pulse // A is Raising-Pulse, B is High > Reverse , pulse-// decrease count-pulse TFLG1 |= 0b10000000 ; // Clear Flag in PT7 } // -// //**** END: Encoder of Motor -****// // CONTROL 2-DC SERVO // void control_2_motors(void){ // Left-Motor : PWM0-PWM1 // Right-Motor: PWM2-PWM3 if (Torque_1 > 185) Torque_1 = 185 ; else if (Torque_1 < -185) Torque_1 = -185 ; // Limitation - 80% PWM // Limitation - 80% PWM if (Torque_2 > 185) Torque_2 = 185 ; else if (Torque_2 < -185) Torque_2 = -185 ; // Limitation - 80% PWM // Limitation - 80% PWM // Robot Turn // if (turn_flag==1){ // - RIGHT-Motor // // Robot Turn left PWMDTY2 = ; // Stop Left-Motor PWMDTY3 = ; // - LEFT-Motor -// if (Torque_1 >= 0){ // FORWARD PWMDTY0 = ; // IN4 >OUT4: 0(volt) - Motor (Right) PWMDTY1 = (int)(Torque_1) ;// IN3 >OUT3: PWM }else{ // REVERSE PWMDTY1 = ; // IN3 >OUT3: 24(volt) - Motor (Right) PWMDTY0 = (int)((-1)*Torque_1) ; // IN4 >OUT4: PWM (24 > 0: Volt) } } // Robot Doesn't Turn // Trang 114 Luận Văn Thạc Sĩ – Điều Khiển Xe Hai Bánh Cân Bằng Và Leo Dốc else if (turn_flag==0){ // - LEFT-Motor // if (Torque_1 >= 0){ // FORWARD PWMDTY0 = ; // IN2 >OUT2: 0(volt) - Motor (Left) PWMDTY1 = (int)(Torque_1) ; // IN1 >OUT1: PWM }else{ // REVERSE PWMDTY1 = ; // IN1 >OUT1: 24(volt) - Motor (Left) PWMDTY0 = (int)((-1)*Torque_1) ; // IN2 >OUT2: PWM (24 > 0: Volt) } // - RIGHT-Motor -// if (Torque_2 >= 0){ // FORWARD PWMDTY3 = ; // IN4 >OUT4: 0(volt) - Motor (Right) PWMDTY2 = (int)(Torque_2) ; // IN3 >OUT3: PWM }else{ // REVERSE PWMDTY2 = ; // IN3 >OUT3: 24(volt) - Motor (Right) PWMDTY3 = (int)((-1)*Torque_2) ; // IN4 >OUT4: PWM (24 > 0: Volt) } } } // // // -// /************ SCAN - KEY - WAKE UP ************/ void KeyBoard(void){ PIFH |= 0xFF; // Clear Flag Key = PTH & 0xFF; // Receive bit: PTH[0-7] } void Key_Init(void){ asm(" sei"); // make atomic TIOS |= 0x10; // PT4 as OC4 : 0b00010000 TCTL1 = TCTL1 & ~0x03 ; // Disconnected at pin PT4 TIE &= 0xEF; // Disarm OC4 : 0b11101111 DDRH = 0x00; // PTH[0-7] are key inputs PPSH = 0x00; // Falling edge at PTH[0-7] PIEH |= 0xFF; // Enable Interrupts PTH[0-7] PIFH |= 0xFF; // Clear Flag KeyBoard(); // Initial read } // -// //********************************* MAIN *******************************// void main(void) { PLL_40_Init(); // 40Mhz - Bus Clock TOC3_Init(); // Enable TOC3 - 20ms Interrupt SCI0_Init_40(115200) ; // Enable SCI0 at 115200 - 40 Mhz Bus Trang 115 Luận Văn Thạc Sĩ – Điều Khiển Xe Hai Bánh Cân Bằng Và Leo Dốc PWM_Init(); TIC_Init(); ADC_Init(); Key_Init() ; LCD_Init() ; // Enable PWM // Enable Timer Input Capture for Hall-Signals and Encoder-Signals // 10-bit ADC , right-justified // Key Wakeup // Enable LCD_module SetupXGATE(); // Enable XGATE with 80Mhz-Bus Clock // -// LCD_goto(1,0); writeLCD_String("A:",2); LCD_goto(2,0); writeLCD_String("1:",2); LCD_goto(2,10); writeLCD_String("2:",2); LCD_goto(3,0); writeLCD_String("3:",2); LCD_goto(4,12); writeLCD_String("T:",2); DDRB = 0xFF ; PORTB = 0x07 ; PORTB_PB0 = 1; DDRM |= 0x03 ; PTM |= 0x03 ; // Output // Two-LEDs are OFF, LED (PB.0) is ON // PM0-PM1 are output // Enable MC33932-A+B /* RF-module Connection -PTS.7 < > Channel A : Forward >> PTS.6 < > Channel B : Reverse Channel C : Stop || PTS.4 < > Channel D : Turn Left-Right */ DDRS &= 0x0F ; // 0xF3 = 0b0000.1111 > PTS.4-7 are inputs for RF set_angle = ; set_speed = ; // Set value: Stand up and no run EnableInterrupts; // -// //*** - LOOP ***// for(;;) { /*** T_sampling = 2.5ms ***/ if (flag_2_5ms == 1){ // 2.5 ms // Read ADC's value to calculate Angle-Y // ROT_Z = ADC_In(0x81); // Gyro-Z-Value is ADC-Channel Y = ADC_In(0x83); // Accelerometer-Y-value is ADC-Channel q_m= (ROT_Z - offset_gyro)/(scale_rot*57.3); // [scale_rot/degree/sec] < > [scale_rot/(pi/180)]= [scale_rot*57.3] (/rad/sec) Trang 116 Luận Văn Thạc Sĩ – Điều Khiển Xe Hai Bánh Cân Bằng Và Leo Dốc // // state_update(q_m); // Update some parameters of KALMAN FILTER //** - * 2.5ms = 7.5ms for CONTROLLER **// if ((count_3==1)||(count_3==4)) { //if ((count==1)||(count==4)) { // T=7.5ms (f=133 Hz) angle_m= asinf((Y - offset_acc_Y)/scale_acc); INPORTANT ANGLE // Arc_sin > Y_angle: is the // Limitation -// // -90(deg) Output: "ANGLE" //* - PID *// speed_wheel_1 = (((float)(count_pulse_1) / Encoder_revol) / Kb) * (2 * pi) ; // Speed (rad/7.5ms) speed_wheel_2 = (((float)(count_pulse_2) / Encoder_revol) / Kb) * (2 * pi) ; speed_wheel = (speed_wheel_1 + speed_wheel_2) / ; // Speed of Robot (rad/7.5ms) count_pulse_1 = ; count_pulse_2 = ; // Clear // position_wheel_1 += speed_wheel_1 * d_wheel / R_wheel; (unit:met/5ms) // position_wheel_2 += speed_wheel_2 * d_wheel / ; // Position += speed * ; speed_error = set_speed - speed_wheel ; // error sigma_speed_error += speed_error ; // intergal of error if (sigma_speed_error > 10) sigma_speed_error=10 ; // Limitation else if (sigma_speed_error < -10) sigma_speed_error=-10 ; u_wheel = Kp_X*speed_error + Ki_X*sigma_speed_error ; // - Limitation -// if (u_wheel > 75) u_wheel = 75 ; else if (u_wheel < -75) u_wheel = -75 ; //* *// //* BACKSTEPPING CONTROLLER FOR ANGLE -*// angle_degree = angle * 57.3 ; // Angle (radian) > Angle (degree) angle_error = set_angle - angle; omega = angle - angle_old ; angle_old = angle; // error (rad) // Omega (rad/ms) // UPDATE Trang 117 Luận Văn Thạc Sĩ – Điều Khiển Xe Hai Bánh Cân Bằng Và Leo Dốc e1 = angle_error ; x1_ref_dot = 0; x1_ref_dot_dot = 0; z1 += e1 ; if (z1 > 16) z1=16 ; else if (z1 < -16) z1=-16 ; // e1 // z1 // Limitation anpha = K1*e1 + C1*z1 + x1_ref_dot ; // anpha if (anpha > 16) anpha=16 ; // Limitation else if (anpha < -16) anpha=-16 ; e2 = anpha - omega; u[1] = angle ; u[2] = omega ; // e2 = anpha - x1_dot = anpha - omega g1 = (0.75*(1+(sinf(u[1]))*(sinf(u[1])))/(Mb*(L*L))+0.75*cosf(u[1])/((2*Mw+Mb)*R*L)) / ((0.75*(Mw*R+Mb*L*cosf(u[1]))*cosf(u[1]))/((2*Mw+Mb)*L)-1); f1_2 = ((-0.75*g*sinf(u[1])/L) + (0.75*Mb*L*sinf(u[1])*cosf(u[1])*((u[2])*(u[2]))/((2*Mw+Mb)*L))) / ((0.75*(Mw*R+Mb*L*cosf(u[1]))*cosf(u[1]))/((2*Mw+Mb)*L)-1) ; u_signal = (-((1+C1-K1*K1)*e1 - (K1+K2)*e2 - K1*C1*z1 + x1_ref_dot_dot - f1_2) / g1) ; // CONTROL SIGNAL // - Limitation -// if(turn_flag==0) { u_signal += u_wheel ; // No Turn // Add for Speed if (u_signal > 160) u_signal = 160 ; else if (u_signal < -160) u_signal = -160 ; } if(turn_flag==1) { // Turn if (u_signal > 140) u_signal = 140 ; else if (u_signal < -140) u_signal = -140 ; } //* -*// if(fabsf(angle_degree) > 20) { u_signal = ; z1 = 0; anpha = 0; e2 = 0; Torque_1 = 0; Torque_2 = 0; position_wheel_1 = 0; position_wheel_2 = 0; speed_error = 0; sigma_speed_error = 0; // Stop Motor if |Angle_degree| > 18 (degree) Trang 118 Luận Văn Thạc Sĩ – Điều Khiển Xe Hai Bánh Cân Bằng Và Leo Dốc } Torque_1 = u_signal ; if (turn_flag==1) Torque_2 = Torque_1 + u_wheel; else Torque_2 = Torque_1 ; control_2_motors() ; // Control DC-Servo > 2-wheels } //** - END OF CONTROLLER **// // - 101 * 2.5ms = 500ms =0.5 (sec) -// if (count_2 == 101){ // For update value to LCD flag_250ms = 1; count_2 = 0; } flag_2_5ms = 0; } // Reset 2.5ms-flag //*** End T_sampling (Ts=2.5ms) -***// /* Control by RF-Module -*/ /* RF-module Connection -PTS.7 < > Channel A : Forward >> PTS.6 < > Channel B : Reverse Channel C : Stop || PTS.4 < > Channel D : Turn Left-Right */ if(PTS_PTS7==1){ // FORWARD (Channel D ) while(PTS_PTS7==1) ; count_forward += ; if(set_angle==0){ set_angle = 0.02 ; // +0.015(rad) > Go Forward set_speed = 0.02 ; LCD_goto(4,9); writeLCD_String("F1",2); } if((set_angle=2)){ set_angle += 0.01 ; // +0.015(rad) > Go Forward set_speed += 0.01 ; if(set_angle>=0.15){ set_angle = 0.15 ; set_speed = 0.15 ; count_forward = 0; } } forward_flag = 1; reverse_flag = 0; stop_flag = 0; Trang 119 Luận Văn Thạc Sĩ – Điều Khiển Xe Hai Bánh Cân Bằng Và Leo Dốc turn_flag = ; } if(PTS_PTS6==1){ // REVERSE (Channel C) while(PTS_PTS6==1) ; count_reverse += ; if(set_angle==0){ set_angle = -0.02 ; // +0.015(rad) > Go Forward set_speed = -0.02 ; LCD_goto(4,9); writeLCD_String("R1",2); } if((set_angle>0)||(count_reverse>=2)){ set_angle -= 0.01 ; // +0.015(rad) > Go Forward set_speed -= 0.01 ; if(set_angle