Xây dựng bộ điều khiển nhúng cho con lắc ngược quay

ĐẠI HỌC QUỐC GIA TP.HCM TRƯỜNG ĐẠI HỌC BÁCH KHOA _o0o _ PHAN VINH HIẾU ĐÊ TÀI XÂY DỰNG BỘ ĐIỀU KHIỂN NHÚNG CHO CON LẮC NGƯỢC QUAY CHUYÊN NGÀNH: T Ự ĐỘNG HÓA MSHV:01507311 LUẬN ÁN THẠC SỸ Tp Hồ Chí Minh, ngày…tháng 06 năm 2010 Luận văn thạc sỹ - Phan Vinh Hiếu CƠNG TRÌNH ĐƯỢC HOÀN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA ĐẠI HỌC QUỐC GIA TP.HỒ CHÍ MINH Cán hướng dẫn khoa học: (ghi rõ họ tên, học hàm, học vị chữ ký) Cán chấm nhận xét 1: (ghi rõ họ tên, học hàm, học vị chữ ký) Cán chấm nhận xét 2: (ghi rõ họ tên, học hàm, học vị chữ ký) Luận văn Thạc Sỹ bảo vệ HỘI ĐỒNG CHẤM BẢO VỆ LUẬN VĂN THẠC SỸ TRƯỜNG ĐẠI HỌC BÁCH KHOA, ngày ….tháng…năm 2010 Luận văn thạc sỹ - Phan Vinh Hiếu ĐẠI HỌC QUỐC GIA TP HCM TRƯỜNG ĐẠI HỌC BÁCH KHOA CỘNG HOÀ XÃ HỘI CHỦ NGHIÃ VIỆT NAM Độc Lập - Tự Do - Hạnh Phúc -oOo Tp HCM, ngày tháng năm 2010 NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên: PHAN VINH HIẾU Giới tính : Nam Ngày, tháng, năm sinh : 09-11-1982 Nơi sinh : TP HỒ CHÍ MINH Chun ngành : TỰ ĐỘNG HĨA Khố (Năm trúng tuyển) : 2007 1- TÊN ĐỀ TÀI: XÂY DỰNG BỘ ĐIỀU KHIỂN NHÚNG CHO CON LẮC NGƯỢC QUAY 2- NHIỆM VỤ LUẬN VĂN: • Xây dựng phần khí điện tử mơ hình lắc ngược quay • Thu thập liệu từ mơ hình thực để ước lượng tham số mơ hình lắc • Xây dựng điều khiển thời gian thực DSP để dựng ngược giũ cân lắc 3- NGÀY GIAO NHIỆM VỤ : 4- NGÀY HOÀN THÀNH NHIỆM VỤ : 5- HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN (Ghi đầy đủ học hàm, học vị ): Nội dung đề cương Luận văn Thạc Sĩ Hội Đồng Chuyên Ngành thông qua CÁN BỘ HƯỚNG DẪN (Họ tên chữ ký) CHỦ NHIỆM BỘ MÔN QUẢN LÝ CHUYÊN NGÀNH (Họ tên chữ ký) Luận văn thạc sỹ - Phan Vinh Hiếu Lời cảm ơn Trước tiên, xin chân thành cảm ơn Thầy Nguyễn Đức Thành người tận tình hướng dẫn truyền đạt kiến thức giúp tơi hồn thành luận án Và quan trọng hết, Ts người gợi mở cho tác giả đề tài, có tính ứng dụng cao, hoàn toàn phù hợp với lực tác giả Bên cạnh tơi xin chân thành cảm ơn đến quý thầy cô môn Điều Khiển Tự Động: thầy Nguyễn Vĩnh Hão, cô Nguyễn Thị Phương Hà, thầy Huỳnh Thái Hoàng, thầy Dương Hoài Nghĩa, thầy Nguyễn Thiện Thành ….đã định hướng hỗ trợ số phần quan trọng cho việc học tập, nghiên cứu phát triển đề tài Tôi xin chân thành cảm ơn bạn cao học khóa 2006 2007 động viên giúp đỡ tơi suốt khóa học Cuối cùng, xin chân thành cảm ơn quan tâm, hỗ trợ, tạo điều kiện động viên vật chất lẫn tinh thần thành viên gia đình suốt thời gian qua Tp.HCM, ngày….tháng….năm 2010 Tác giả PHAN VINH HIẾU Luận văn thạc sỹ - Phan Vinh Hiếu Tóm tắt luận văn thạc sỹ Con lắc ngược quay đối tượng đại diện cho lớp đối tượng có độ phi tuyến cao không ổn định Với ưu điểm hệ thống khí khơng q phức tạp (so với lắc ngược đặt xe) lắc ngược quay sử dụng rộng rãi việc giảng dạy nghiên cứu lý thuyết điều khiển tự động xây dựng điều khiển số Hệ thống bao gồm que kim loại (con lắc) quay quanh trục thẳng đứng Que kim loại gắn gián tiếp vào đầu cánh tay nằm ngang thông qua encoder để đo góc, đầu cịn lại cánh tay gắn vào trục quay động servo DC đặt thẳng đứng để cánh tay quay mặt phẳng nằm ngang Mục tiêu đề tài thu thập liệu ước lượng tham số lắc phương pháp Bình Phương Cực Tiểu, sau xây dựng luật điều khiển dựng lắc từ vị trí thẳng đứng hướng xuống đến vị trí thẳng đứng hướng lên phương pháp K Futara - M Yamakita đề xuất giữ lắc cân vị trí phương pháp Điều Khiển Đặt Cực, LQR Luật điều khiển xây dựng DSP xử lý dấu chấm động TMS320f28335 Ưu điểm sử dụng DSP để thực thi điều khiển là: kích thước nhỏ gọn, giá thành rẻ, sử dụng linh hoạt dễ dàng ứng dụng vào thực tế Abstract The rotary inverted pendulum system is a highly nonlinear and unstable system, The mechanism of this system is not complicated Because of this, this system provides a platform for verifying the effectiveness of different control schemes The rotary inverted pendulum consists of a rigid rod (pendulum) rotating in a vertical plane The rigid rod is attached to a pivot arm that is mounted on the shaft of a DC motor such that the pivot arm can rotate in the horizontal plan The thesis objective is collect data and use Least Squares Estimation Methods to estimate System’s Parameter.After that, Building a controller which will swing up the pendulum from the downward position and to balance the pendulum in the vertical-upright position The Pole Replacement and LQR control scheme are used to achieve balancing control, and the controller proposed by K Futara and M Yamakita is used to swing up the pendulum The control law is implemented through a IC TMS320f28335 floating point digital signal processor The key features of this DSP-Based stand-alone control system include portability, minimizing the hardware size and saving cost Luận văn thạc sỹ - Phan Vinh Hiếu Mục lục Chương 1: Tổng quan 01 1.1 Đặt vấn đề .01 1.2 Tổng quan đề tài .02 1.3 Tóm tắt nội dung chương 06 Chương 2: Nhiệm vụ luận văn 07 Chương 3: Mơ hình hóa ước lượng tham số .08 3.1 Mơ hình hố hệ thống lắc ngược quay 08 3.2 Ước lượng thông số hệ thống 13 3.3 Kỹ thuật kiểm chứng mơ hình 20 Chương 4: Thiết kế điều khiển 21 4.1 Giới thiệu 21 4.2 Thiết kế điều khiển Lật ngược lắc 21 4.3 Thiết kế điều khiển Cân 22 4.4 Mô điều khiển Simulink 29 4.5 So sánh hai điều khiển Đặt cực LQR 38 Chương 5: Ước lượng thông số từ liệu thu thập .39 5.1 Giới thiệu .39 5.2 Lập trình DSP tạo tín hiệu kích thu thập liệu 40 5.3 Tiến hành thu thập liệu nhận dạng 45 5.4 Đánh giá kết ước lượng tham số 46 Chương 6: Xây dựng điều khiển thời gian thực DSP 57 Luận văn thạc sỹ - Phan Vinh Hiếu 6.1 Giới thiệu 57 6.2 Xây dựng điều khiển Lật ngược giữ Cân lắc DSP Phương pháp đặt cực 57 6.3 Xây dựng điều khiển Lật ngược giữ Cân lắc DSP Phương pháp LQR rời rạc 68 6.4 So sánh hai điều khiển Đặt cực LQR 71 Chương 7: Kết luận hướng phát triển đề tài ……… .68 7.1 Kết luận……… 72 7.2 Hướng phát triển đề tài 73 Phụ lục 74 Phần A: Thành lập mơ hình tốn học phi tuyến .74 A.1 Hệ thống theo toạ độ 74 A.2 Phương trình Lagrange chuyển động(các hệ thống quay) 75 A.3 Mơ hình động lực học (vị trí thẳng đứng) 75 A.4 Mơ hình động lực học(theo phương hướng xuống) 78 Phần B: Tuyến tính hố mơ hình tốn học phi tuyến 80 B.1 Tuyến tính hố mơ hình tốn học phi tuyến 80 B.2 Mơ hình tuyến tính (theo phương thẳng đứng) 80 Phần C: Kết ước lượng sử dụng Phương Pháp Bình Phương Cực Tiểu C.1 Kết ước lượng sử dụng Phương Pháp 85 C.2 Kết ước lượng sử dụng Phương Pháp 88 Phần D: Thiết kế khí 91 Luận văn thạc sỹ - Phan Vinh Hiếu Phần E: Thiết kế điện tử 94 E.1 Mạch nguồn 94 E.2 Mạch cầu H lái động 94 E.3 Mạch điều khiển 96 E.4 Giới thiệu Encoder 100 E.5 Module kết nối encoder 100 Phần F: Phần mềm lập trình 102 F.1 Giới thiệuTarget For TI C2000 102 F.2 Sử dụng Target Preferences Library 103 F.3 Sử dụng Chip Support Library 104 F.4 Code Composer Studio 105 Phần G: Mã lập trình G.1 Chương trình Matlab ước lượng tham số mơ hình 107 G.2 Chương trình CCS chạy độc lập DSP cân lắc 112 Tài liệu tham khảo 126 Lý Lịch trích ngang: 127 Danh mục hàm, ký hiệu, từ viết tắt • Ký hiệu • Từ viết tắt LQR: Linear-quadratic regulator Tồn phương tuyến tính DSP: Digital signal processing Bộ xử lý tín hiệu số CCS: Code Composer Studio Trình soạn thảo mã lập trình cho DSP Danh mục hình vẽ, đồ thị, bảng biểu Luận văn thạc sỹ - Phan Vinh Hiếu Hình 1.1 Sơ đồ khối hệ thống điều khiển máy tính 01 Hình 1.2 Sơ đồ khối hệ thống điều khiển hệ thống nhúng 02 Hình 1.6 Sơ đồ khối hệ lắc ngược quay 04 Hình 1.7 Cấu trúc lắc ngược bậc tự do tác giả chế tạo 05 Hình 3.1 Hệ trục tọa độ lắc 08 Hình 3.2 Nguyên lý nhận dạng hệ thống .13 Hình 3.3 Tín hiệu nhận dạng mẫu 16 Hình 3.4 Mơ hình Simulink kiểm chứng kết ước lượng 20 Hình 4.1 Sơ đồ điều khiển tổng quát .21 Hình 4.2 Trường vector chiếu .22 Hình 4.3 Sơ đồ điều khiển hệ hở 23 Hình 4.4 Điều khiển đặt cực 23 Hình 4.6 Sơ đồ khối điều khiển rời rạc .26 Hình 4.7 Sơ đồ điều khiển LQR 27 Hình 4.8 Mơ hình Simulink mơ điều khiển cân lắc sử dụng phương pháp đặt cực 30 Hình 4.10 Kết đáp ứng điều khiển cân lắc mô Simulink phương pháp đặt cực 31 Hình 4.11 Mơ hình Simulink mô điều khiển cân lắc vị trí cánh tay bám theo tín hiệu đặt sử dụng phương pháp đặt cực 32 Hình 4.12 Kết đáp ứng điều khiển cân lắc vị trí cánh tay bám theo tín hiệu đặt mô Simulink phương pháp đặt cực 33 Hình 4.13 Mơ hình Simulink mơ điều khiển cân lắc sử dụng phương pháp LQR 34 Luận văn thạc sỹ - Phan Vinh Hiếu Hình 4.14.Kết đáp ứng điều khiển cân lắc mô 35 Simulink phương pháp LQR Hình 4.15.Mơ hình Simulink mơ điều khiển cân vị trí cánh tay 36 bám theo tín hiệu đặt sử dụng phương pháp LQR Hình 4.16.Kết đáp ứng điều khiển cân lắc vị trí cánh tay bám theo tín hiệu đặt mô Simulink phương pháp LQR 37 Hình 5.1 Nguyên lý nhận dạng hệ thống 39 Hình 5.2 Chương trình DSP tạo tín hiệu kích thu thập liệu 40 Hình 5.3 Cấu trúc bên khối Motor Control Signal………………… 40 Hình 5.7 Cấu trúc bên khối Arm Velocity Signal ………………… 42 Hình 5.10 Cấu trúc bên khối Pen Position Signal 43 Hình 5.12 Cấu trúc bên khối RS232 44 Hình 5.13 Đồ thị đáp ứng 45 Hình 5.14 Đồ thị đáp ứng đánh gía kết ước lượng 49 Hình 6.1 Chương trình điều khiển Lật ngược, Dữ cân lắc DSP sử dụng phương pháp đặt cực 57 Hình 6.2 Sơ đồ khối chương trình khối Swing Up-State Feeback Controller 58 Hình 6.3 Khối Arm Module 60 Hình 6.4 Khối Pen Module 60 Hình 6.5 Khối Motor Module .61 Hình 6.7 Đồ thị đáp ứng Swing Up hệ thống ………………………… 62 Hình 6.8 Đồ thị đáp ứng cân hệ thống với hệ số K1d 63 Hình 6.9 Đồ thị đáp ứng cân hệ thống với hệ số K2d 64 Luận Văn Thạc Sỹ Học Viên: Phan Vinh Hiếu GVHD : TS Nguyễn Đức Thành PHỤ LỤC G MÃ LẬP TRÌNH: G.1 Chương trình Matlab ước lượng tham số mơ hình: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %PHAN VINH HIEU %HCM UNIVERSITY OF TECHNOLOGY %UOC LUONG THONG SO SU DUNG PHUONG PHAP 1&2 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% A = read_dat('Data18SProcess\system.sdt'); if (A == 0) T = 0; A_REV = 0; B_REV = 0; return; end T = A(1); A_REV = A(2); B_REV = A(3); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %getraw err_data = 5; A = read_dat('Data18SProcess\HDATA01S.DAT'); if (A == 0) u_r = 0; da_r = 0; b_r = 0; return; end A(1) = []; x = fix(length(A)/3)-err_data; y = 4:3:(3*x); u_r = A(y); da_r = A(y+1); b_r = A(y+2); da_r = da_r*2*pi/(A_REV*T); b_r = b_r*2*pi/B_REV; %DOI SANG RADIAN %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 %Data Estimation bf=0.15; %cac he so cua bo loc iir af=[1 -0.85]; u da b = filt_sig(bf,af,u_r); = filt_sig(bf,af,da_r); = filt_sig(bf,af,b_r); - 104 - %Loc du lieu Luận Văn Thạc Sỹ Học Viên: Phan Vinh Hiếu GVHD : TS Nguyễn Đức Thành G.2 Chương trình CCS chạy độc lập DSP lật ngược giữ cân lắc: /* * File: DATA_DISCRETE_INCREMENTAL_SWINGUP_FIVE_STATE_CON_ARMPOS5.c * * Real-Time Workshop code generated for Simulink model DATA_DISCRETE_INCREMENTAL_SWINGUP_FIVE_STATE_CON_ARMPOS5 * * Model version : 1.478 * Real-Time Workshop file version : 7.3 (R2009a) 15Jan-2009 * Real-Time Workshop file generated on : Wed May 26 17:30:46 2010 * TLC version : 7.3 (Jan 18 2009) * C/C++ source code generated on : Wed May 26 17:30:46 2010 * * Target selection: ccslink_ert.tlc * Embedded hardware selection: Texas Instruments->C2000 * Code generation objectives: Unspecified * Validation result: Not run */ #include "DATA_DISCRETE_INCREMENTAL_SWINGUP_FIVE_STATE_CON_ARMPOS5.h" #include "DATA_DISCRETE_INCREMENTAL_SWINGUP_FIVE_STATE_CON_ARMPOS5_pr ivate.h" /* Block signals (auto storage) */ BlockIO_DATA_DISCRETE_INCREMENT DATA_DISCRETE_INCREMENTAL_SWI_B; /* Block states (auto storage) */ D_Work_DATA_DISCRETE_INCREMENTA DATA_DISCRETE_INCREMENTAL_DWork; /* Real-time model */ RT_MODEL_DATA_DISCRETE_INCREMEN DATA_DISCRETE_INCREMENTAL_SW_M_; RT_MODEL_DATA_DISCRETE_INCREMEN *DATA_DISCRETE_INCREMENTAL_SW_M = &DATA_DISCRETE_INCREMENTAL_SW_M_; /* Model step function */ void DATA_DISCRETE_INCREMENTAL_SWINGUP_FIVE_STATE_CON_ARMPOS5_ste p(void) { /* local block i/o variables */ real_T rtb_PWMGain; uint8_T rtb_TmpHiddenBufferAtSCITransmi[17]; - 105 - Luận Văn Thạc Sỹ Học Viên: Phan Vinh Hiếu GVHD : TS Nguyễn Đức Thành { int16_T eml_mode; real_T rtb_ARMGain; real_T rtb_y; int32_T rtb_Add_c; int32_T rtb_Add_d; /* DiscretePulseGenerator: '/Pulse Generator' */ rtb_PWMGain = ((real_T)DATA_DISCRETE_INCREMENTAL_DWork.clockTickCounter < DATA_DISCRETE_INCREMENTAL_SWI_P.PulseGenerator_Duty) && (DATA_DISCRETE_INCREMENTAL_DWork.clockTickCounter >= 0L) ? DATA_DISCRETE_INCREMENTAL_SWI_P.PulseGenerator_Amp : 0.0; if ((real_T)DATA_DISCRETE_INCREMENTAL_DWork.clockTickCounter >= DATA_DISCRETE_INCREMENTAL_SWI_P.PulseGenerator_Period - 1.0) { DATA_DISCRETE_INCREMENTAL_DWork.clockTickCounter = 0L; } else { DATA_DISCRETE_INCREMENTAL_DWork.clockTickCounter = DATA_DISCRETE_INCREMENTAL_DWork.clockTickCounter + 1L; } /* Gain: '/ARM Gain' */ rtb_ARMGain = DATA_DISCRETE_INCREMENTAL_SWI_P.ARMGain_Gain * rtb_PWMGain; /* S-Function Block: /eQEP3 (c280xqep) */ { DATA_DISCRETE_INCREMENTAL_SWI_B.eQEP3 = EQep1Regs.QPOSCNT;//eQEP Position Counter } /* S-Function "ARMENCODER_wrapper" Block: /SFunction Builder */ ARMENCODER_Outputs_wrapper(&DATA_DISCRETE_INCREMENTAL_SWI_B eQEP3, &DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder7 ); /* Sum: '/Add' */ rtb_ARMGain += DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder7; /* Sum: '/Add' incorporates: * UnitDelay: '/Unit Delay1' */ - 106 - Luận Văn Thạc Sỹ Học Viên: Phan Vinh Hiếu GVHD : TS Nguyễn Đức Thành rtb_Add_c = (int32_T)DATA_DISCRETE_INCREMENTAL_SWI_B.eQEP3 - (int32_T) DATA_DISCRETE_INCREMENTAL_DWork.UnitDelay1_DSTATE_k; /* S-Function Block: /eQEP2 (c280xqep) */ { DATA_DISCRETE_INCREMENTAL_SWI_B.eQEP2 = EQep2Regs.QPOSCNT;//eQEP Position Counter } /* Gain: '/Gain' */ DATA_DISCRETE_INCREMENTAL_SWI_B.Gain = DATA_DISCRETE_INCREMENTAL_SWI_P.Gain_Gain * DATA_DISCRETE_INCREMENTAL_SWI_B.eQEP2; /* S-Function "PENENCODER2_wrapper" Block: /SFunction Builder */ PENENCODER2_Outputs_wrapper(&DATA_DISCRETE_INCREMENTAL_SWI_B Gain, &DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder4_o1, &DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder4_o2 ); /* Sum: '/Add' incorporates: * UnitDelay: '/Unit Delay1' */ rtb_Add_d = (int32_T)DATA_DISCRETE_INCREMENTAL_SWI_B.Gain - (int32_T) DATA_DISCRETE_INCREMENTAL_DWork.UnitDelay1_DSTATE_i; /* Embedded MATLAB: '/SWING UP STATE FEEBACK CONTROLLER' incorporates: * UnitDelay: '/Unit Delay1' */ /* Embedded MATLAB Function 'SWING UP STATE FEEBACK CONTROLLER': ':1' */ /* ':1:3' */ rtb_y = 0.0; /* ':1:4' */ eml_mode = 0; if (fabs(DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder4_o2) < 300.0) { /* ':1:7' */ /* ':1:8' */ eml_mode = 1; /* ':1:9' */ rtb_y = 0.0; } - 107 - Luận Văn Thạc Sỹ Học Viên: Phan Vinh Hiếu GVHD : TS Nguyễn Đức Thành if (eml_mode == 0) { /* ':1:22' */ if ((DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder4_o1 = 1700.0)) { /* ':1:23' */ if (rtb_Add_d = 0.0) && (DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder4_o1 = 0L) { /* ':1:30' */ /* ':1:31' */ rtb_y = 125.0; } else { /* ':1:33' */ rtb_y = 0.0; } } } } else if (eml_mode == 1) { /* ':1:36' */ if ((fabs(DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder4_o2) 20.0)) { /* ':1:37' */ /* ':1:38' */ 3.1415926535897931E+000 / 80.0 * 172.5) + 0.6 * DATA_DISCRETE_INCREMENTAL_DWork.UnitDelay1_DSTATE; if (rtb_ARMGain < 0.0) { rtb_y = ceil(rtb_ARMGain - 0.5); } else { rtb_y = floor(rtb_ARMGain + 0.5); - 108 - Luận Văn Thạc Sỹ Học Viên: Phan Vinh Hiếu GVHD : TS Nguyễn Đức Thành } } else { if ((fabs(DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder4_o2) = 0.0)) { /* ':1:43' */ /* ':1:44' */ rtb_ARMGain = (((rtb_ARMGain * 2.0 * 3.1415926535897931E+000 / 4096.0 * DATA_DISCRETE_INCREMENTAL_DWork.UnitDelay1_DSTATE; if (rtb_ARMGain < 0.0) { rtb_y = ceil(rtb_ARMGain - 0.5); } else { rtb_y = floor(rtb_ARMGain + 0.5); } } } } else { /* ':1:53' */ rtb_y = 0.0; } /* Embedded MATLAB: '/PROCESS U CONTROL' */ /* Embedded MATLAB Function 'MOTOR MODULE1/PROCESS U CONTROL': ':1' */ /* ':1:4' */ DATA_DISCRETE_INCREMENTAL_SWI_B.direc = 0.0; if (rtb_y > 0.0) { /* ':1:5' */ /* ':1:6' */ rtb_ARMGain = rtb_y * 64.0; /* ':1:7' */ DATA_DISCRETE_INCREMENTAL_SWI_B.brake = 1.0; /* ':1:8' */ DATA_DISCRETE_INCREMENTAL_SWI_B.direc = 1.0; } else if (rtb_y < 0.0) { /* ':1:9' */ /* ':1:10' */ rtb_ARMGain = fabs(rtb_y * 64.0); /* ':1:11' */ DATA_DISCRETE_INCREMENTAL_SWI_B.brake = 1.0; /* ':1:12' */ DATA_DISCRETE_INCREMENTAL_SWI_B.direc = 0.0; } else { - 109 - Luận Văn Thạc Sỹ Học Viên: Phan Vinh Hiếu GVHD : TS Nguyễn Đức Thành /* ':1:14' */ rtb_ARMGain = rtb_y; /* ':1:15' */ DATA_DISCRETE_INCREMENTAL_SWI_B.brake = 0.0; /* neu u=0 thi thang dong co */ } /* S-Function Block: /GPIO 59 Direction Signal 0-C+ //// 1-C- (c280xgpio_do) */ { GpioDataRegs.GPBSET.bit.GPIO59 = (DATA_DISCRETE_INCREMENTAL_SWI_B.direc != 0); GpioDataRegs.GPBCLEAR.bit.GPIO59 = !(DATA_DISCRETE_INCREMENTAL_SWI_B.direc != 0); } /* S-Function Block: /GPIO61 Output1 Meter (c280xgpio_do) */ { GpioDataRegs.GPBSET.bit.GPIO61 = (DATA_DISCRETE_INCREMENTAL_SWI_B.brake != 0); GpioDataRegs.GPBCLEAR.bit.GPIO61 = !(DATA_DISCRETE_INCREMENTAL_SWI_B.brake != 0); } /* S-Function Block: /GPIO62 Output1 Meter1 (c280xgpio_do) */ { GpioDataRegs.GPBSET.bit.GPIO62 = (DATA_DISCRETE_INCREMENTAL_SWI_B.direc != 0); GpioDataRegs.GPBCLEAR.bit.GPIO62 = !(DATA_DISCRETE_INCREMENTAL_SWI_B.direc != 0); } /* S-Function Block: /PIO 58 1//OFF Brake Brake (c280xgpio_do) */ { GpioDataRegs.GPBSET.bit.GPIO58 = (DATA_DISCRETE_INCREMENTAL_SWI_B.brake != 0); GpioDataRegs.GPBCLEAR.bit.GPIO58 = !(DATA_DISCRETE_INCREMENTAL_SWI_B.brake != 0); } - 110 - 0//ON Luận Văn Thạc Sỹ Học Viên: Phan Vinh Hiếu GVHD : TS Nguyễn Đức Thành /* Gain: '/PWM Gain' */ rtb_PWMGain = DATA_DISCRETE_INCREMENTAL_SWI_P.PWMGain_Gain * rtb_ARMGain; /* Update CMPA value for ePWM1 */ { EPwm1Regs.CMPA.half.CMPA = (uint16_T)(rtb_PWMGain); } /* DataTypeConversion: '/Data Type Conversion1' */ if (rtIsNaN(rtb_y) || rtIsInf(rtb_y)) { rtb_ARMGain = 0.0; } else { rtb_ARMGain = fmod(rtb_y, 65536.0); } DATA_DISCRETE_INCREMENTAL_SWI_B.DataTypeConversion1 = rtb_ARMGain < 0.0 ? -((int16_T)(uint16_T)(-rtb_ARMGain)) : (int16_T)(uint16_T)rtb_ARMGain; /* S-Function "IntToStr4_wrapper" Block: /S-Function Builder */ IntToStr4_Outputs_wrapper (&DATA_DISCRETE_INCREMENTAL_SWI_B.DataTypeConversion1, DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder2 ); /* S-Function "ARMENCODERSIGNAL_wrapper" Block: /SFunction Builder */ ARMENCODERSIGNAL_Outputs_wrapper(&DATA_DISCRETE_INCREMENTAL_ SWI_B.eQEP3, &DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder1 ); /* S-Function "IntToStr5_wrapper" Block: /S-Function Builder */ IntToStr5_Outputs_wrapper(&DATA_DISCRETE_INCREMENTAL_SWI_B.S FunctionBuilder1, DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder2_a ); /* S-Function "PENENCODER5_wrapper" Block: /SFunction Builder */ PENENCODER5_Outputs_wrapper(&DATA_DISCRETE_INCREMENTAL_SWI_B Gain, &DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder3_o1, &DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder3_o2 ); /* S-Function "IntToStr5_wrapper" Block: /S-Function Builder */ IntToStr5_Outputs_wrapper - 111 - Luận Văn Thạc Sỹ Học Viên: Phan Vinh Hiếu GVHD : TS Nguyễn Đức Thành (&DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder3_o2, DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder1_g ); /* SignalConversion: '/TmpHiddenBufferAtSCI Transmit1Inport1' incorporates: * Constant: '/Constant2' * Constant: '/Constant3' * Constant: '/Constant5' */ rtb_TmpHiddenBufferAtSCITransmi[0] = DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder2[0]; rtb_TmpHiddenBufferAtSCITransmi[1] = DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder2[1]; rtb_TmpHiddenBufferAtSCITransmi[2] = DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder2[2]; rtb_TmpHiddenBufferAtSCITransmi[3] = DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder2[3]; rtb_TmpHiddenBufferAtSCITransmi[4] = DATA_DISCRETE_INCREMENTAL_SWI_P.Constant3_Value; for (eml_mode = 0; eml_mode < 5; eml_mode++) { rtb_TmpHiddenBufferAtSCITransmi[eml_mode + 5] = DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder2_a[eml_mode ]; } rtb_TmpHiddenBufferAtSCITransmi[10] = DATA_DISCRETE_INCREMENTAL_SWI_P.Constant5_Value; for (eml_mode = 0; eml_mode < 5; eml_mode++) { rtb_TmpHiddenBufferAtSCITransmi[eml_mode + 11] = DATA_DISCRETE_INCREMENTAL_SWI_B.SFunctionBuilder1_g[eml_mode ]; } rtb_TmpHiddenBufferAtSCITransmi[16] = DATA_DISCRETE_INCREMENTAL_SWI_P.Constant2_Value; /* S-Function Block: /SCI Transmit1 (c28xsci_tx) */ { scia_xmit((char*)rtb_TmpHiddenBufferAtSCITransmi, 17); } /* DiscretePulseGenerator: '/Pulse Generator1' */ DATA_DISCRETE_INCREMENTAL_SWI_B.PulseGenerator1 = ((real_T) DATA_DISCRETE_INCREMENTAL_DWork.clockTickCounter_k < DATA_DISCRETE_INCREMENTAL_SWI_P.PulseGenerator1_Duty) && (DATA_DISCRETE_INCREMENTAL_DWork.clockTickCounter_k >= 0L) ? - 112 - Luận Văn Thạc Sỹ Học Viên: Phan Vinh Hiếu GVHD : TS Nguyễn Đức Thành DATA_DISCRETE_INCREMENTAL_SWI_P.PulseGenerator1_Amp : 0.0; if ((real_T)DATA_DISCRETE_INCREMENTAL_DWork.clockTickCounter_k >= DATA_DISCRETE_INCREMENTAL_SWI_P.PulseGenerator1_Period 1.0) { DATA_DISCRETE_INCREMENTAL_DWork.clockTickCounter_k = 0L; } else { DATA_DISCRETE_INCREMENTAL_DWork.clockTickCounter_k = DATA_DISCRETE_INCREMENTAL_DWork.clockTickCounter_k + 1L; } /* S-Function Block: /OVERRUN CONTROL (c280xgpio_do) */ { GpioDataRegs.GPBSET.bit.GPIO60 = (DATA_DISCRETE_INCREMENTAL_SWI_B.PulseGenerator1 != 0); GpioDataRegs.GPBCLEAR.bit.GPIO60 = !(DATA_DISCRETE_INCREMENTAL_SWI_B.PulseGenerator1 != 0); } /* Update for UnitDelay: '/Unit Delay1' */ DATA_DISCRETE_INCREMENTAL_DWork.UnitDelay1_DSTATE_k = DATA_DISCRETE_INCREMENTAL_SWI_B.eQEP3; /* Update for UnitDelay: '/Unit Delay1' */ DATA_DISCRETE_INCREMENTAL_DWork.UnitDelay1_DSTATE_i = DATA_DISCRETE_INCREMENTAL_SWI_B.Gain; /* Update for UnitDelay: '/Unit Delay1' */ DATA_DISCRETE_INCREMENTAL_DWork.UnitDelay1_DSTATE = rtb_y; } } /* Model initialize function */ void DATA_DISCRETE_INCREMENTAL_SWINGUP_FIVE_STATE_CON_ARMPOS5_ini tialize (boolean_T firstTime) { (void)firstTime; /* Registration code */ /* initialize non-finites */ rt_InitInfAndNaN(sizeof(real_T)); - 113 - Luận Văn Thạc Sỹ Học Viên: Phan Vinh Hiếu GVHD : TS Nguyễn Đức Thành /* initialize error status */ rtmSetErrorStatus(DATA_DISCRETE_INCREMENTAL_SW_M, (NULL)); /* block I/O */ (void) memset(((void *) &DATA_DISCRETE_INCREMENTAL_SWI_B),0, sizeof(BlockIO_DATA_DISCRETE_INCREMENT)); /* states (dwork) */ (void) memset((void *)&DATA_DISCRETE_INCREMENTAL_DWork, 0, sizeof(D_Work_DATA_DISCRETE_INCREMENTA)); /* Start for DiscretePulseGenerator: '/Pulse Generator' */ DATA_DISCRETE_INCREMENTAL_DWork.clockTickCounter = 0L; config_QEP_eQEP1(65535U, 0, 0, 0, 0, 3072, 8, 32768, 119, 0); config_QEP_eQEP2(65535U, 0, 0, 0, 0, 3072, 8, 32768, 119, 0); EALLOW; GpioCtrlRegs.GPBPUD.all = 0U; GpioCtrlRegs.GPBMUX2.all &= 4282384383U; GpioCtrlRegs.GPBDIR.all |= 134217728U; EDIS; EALLOW; GpioCtrlRegs.GPBPUD.all = 0U; GpioCtrlRegs.GPBMUX2.all &= 4093640703U; GpioCtrlRegs.GPBDIR.all |= 536870912U; EDIS; EALLOW; GpioCtrlRegs.GPBPUD.all = 0U; GpioCtrlRegs.GPBMUX2.all &= 3489660927U; GpioCtrlRegs.GPBDIR.all |= 1073741824U; EDIS; EALLOW; GpioCtrlRegs.GPBPUD.all = 0U; GpioCtrlRegs.GPBMUX2.all &= 4291821567U; GpioCtrlRegs.GPBDIR.all |= 67108864U; EDIS; /*** Initialize ePWM1 modules ***/ { EPWMPARAMS EPwm1Params; /* Setup Time-Base (TB) Submodule */ EPwm1Params.TBPRD = 64000; EPwm1Params.TBCTL_CTRMODE = 0; EPwm1Params.TBCTL_SYNCOSEL = 3; EPwm1Params.TBCTL_PHSEN = 0; EPwm1Params.TBCTL_PHSDIR = 0; EPwm1Params.TBPHS = 0; EPwm1Params.TBCTL_HSPCLKDIV = 0; - 114 - Luận Văn Thạc Sỹ Học Viên: Phan Vinh Hiếu GVHD : TS Nguyễn Đức Thành EPwm1Params.TBCTL_CLKDIV = 0; /* Setup Counter_Compare (CC) Submodule */ EPwm1Params.CMPCTL_LOADAMODE = 0; EPwm1Params.CMPCTL_LOADBMODE = 0; EPwm1Params.CMPA = 0; EPwm1Params.CMPB = 64000; /* Setup Action-Qualifier (AQ) Submodule */ EPwm1Params.AQCTLA = 36; EPwm1Params.AQCTLB = 264; EPwm1Params.AQCSFRC_CSFA = 0; EPwm1Params.AQCSFRC_CSFB = 0; EPwm1Params.AQCSFRC_RLDCSF = 0; /* Setup Dead-Band Generator (DB) Submodule */ EPwm1Params.DBCTL_OUT_MODE = 0; EPwm1Params.DBCTL_IN_MODE = 0; EPwm1Params.DBCTL_POLSEL = 0; EPwm1Params.DBRED = 0.0; EPwm1Params.DBFED = 0.0; /* Setup Event-Trigger (ET) Submodule */ EPwm1Params.ETSEL_SOCAEN = 0; EPwm1Params.ETSEL_SOCASEL = 1; EPwm1Params.ETPS_SOCAPRD = 1; EPwm1Params.ETSEL_SOCBEN = 0; EPwm1Params.ETSEL_SOCBSEL = 1; EPwm1Params.ETPS_SOCBPRD = 1; EPwm1Params.ETSEL_INTEN = 0; EPwm1Params.ETSEL_INTSEL = 1; EPwm1Params.ETPS_INTPRD = 1; /* Setup PWM-Chopper (PC) Submodule */ EPwm1Params.PCCTL_CHPEN = 0; EPwm1Params.PCCTL_CHPFREQ = 0; EPwm1Params.PCCTL_OSHTWTH = 0; EPwm1Params.PCCTL_CHPDUTY = 0; /* Setup Trip-Zone (TZ) Submodule */ EPwm1Params.TZSEL = 0; EPwm1Params.TZCTL_TZA = 1; EPwm1Params.TZCTL_TZB = 2; EPwm1Params.TZEINT_OST = 0; EPwm1Params.TZEINT_CBC = 0; /* Initial ePWM1 */ config_ePWMRegs(&EPwm1Regs, &EPwm1Params); } /* Start for DiscretePulseGenerator: '/Pulse Generator1' */ DATA_DISCRETE_INCREMENTAL_DWork.clockTickCounter_k = 0L; EALLOW; GpioCtrlRegs.GPBPUD.all = 0U; GpioCtrlRegs.GPBMUX2.all &= 4244635647U; GpioCtrlRegs.GPBDIR.all |= 268435456U; EDIS; /* InitializeConditions for UnitDelay: '/Unit Delay1' */ - 115 - Luận Văn Thạc Sỹ Học Viên: Phan Vinh Hiếu GVHD : TS Nguyễn Đức Thành DATA_DISCRETE_INCREMENTAL_DWork.UnitDelay1_DSTATE_k = DATA_DISCRETE_INCREMENTAL_SWI_P.UnitDelay1_X0_n; /* InitializeConditions for UnitDelay: '/Unit Delay1' */ DATA_DISCRETE_INCREMENTAL_DWork.UnitDelay1_DSTATE_i = DATA_DISCRETE_INCREMENTAL_SWI_P.UnitDelay1_X0_i; /* InitializeConditions for UnitDelay: '/Unit Delay1' */ DATA_DISCRETE_INCREMENTAL_DWork.UnitDelay1_DSTATE = DATA_DISCRETE_INCREMENTAL_SWI_P.UnitDelay1_X0; } /* Model terminate function */ void DATA_DISCRETE_INCREMENTAL_SWINGUP_FIVE_STATE_CON_ARMPOS5_ter minate(void) { /* (no terminate code required) */ } /* File trailer for Real-Time Workshop generated code * * [EOF] */ TÀI LIỆU THAM KHẢO [1] S.H Ang and M.B.Tan, Final Year Project 4029 Report, 1997 [2] T.L.Yong, “State Space Predictive Control of Inverted Pendulum”,1998 [3] K.Futura and M Yamakita,”Swing up control of inverted pendulum using pseudo-state feedback”, Proc Instn Mech Engrs Vol 206, 1992 [4] H.L Langhaar,”Energy Methods in Applied Mechanics”, Jonh Wiley and Sons Inc, pp 233-249,1962 [5] K.Ogata, “Electromechanical Systems”, ‘Modern Control Egineering’,Prentice Hall, pp 118-121, 1995 [6] K.V.Ling.”Parameter Estimation of the KRI-PP300 Inverted Pendulum”,1998 - 116 - Luận Văn Thạc Sỹ Học Viên: Phan Vinh Hiếu GVHD : TS Nguyễn Đức Thành [7] S.c Tan and K.V.Ling, ”Parameters Indentification of a Rotary Inverted Pendulum”, 1998 [8] Huỳnh Thái Hồng “Lý Thuyết Điều Khiển Thơng Minh”, Nhà Xuất Bản Đại Học Quốc Gia 2006 [9] Nguyễn Thị Phương Hà, Huỳnh Thái Hoàng “Lý Thuyết Điều Khiển Tự Động”, Nhà Xuất Bản Đại Học Quốc Gia 2006 [10] Nguyễn Thị Phương Hà , “Lý Thuyết Điều Khiển Hiện Đại” , Nhà Xuất Bản Đại Học Quốc Gia 2008 [11] Tim Wescott “Applied Control Theory for embedded system” , 2006, Elsevier Inc [12] OReilly~2005.05 “Designing Embedded Hardware ~ 2nd Edition “– “ LÝ LỊCH TRÍCH NGANG Họ tên : PHAN VINH HIẾU Phái Ngày sinh : 09-11-1982 Nơi sinh : TP HỒ CHÍ MINH Địa liên lạc : Điện thoại : Nam 63/9 CMT8 F5 Q TÂN BÌNH TP HCM : 0983053941- (08) 62972784 QUÁ TRÌNH ĐÀO TẠO 2000-2004: sinh viên chuyên ngành Vật Lý Điện Tử trường Đại Học Khoa Học Tự Nhiên - 117 - Luận Văn Thạc Sỹ Học Viên: Phan Vinh Hiếu GVHD : TS Nguyễn Đức Thành 2007-2010 : Học viên Cao Học trường Đại Học Bách Khoa chuyên ngành Tự Động Hóa Q TRÌNH CƠNG TÁC Từ 2005-2006: Kỹ sư Tự Động Hóa cho Cơng Ty Thang Máy Tồn Tâm Từ 2006-đến : Giáo viên Trung Tâm Điện Tử-Máy Tính thuộc Đại Học Khoa Học Tự Nhiện TP HCM THÀNH TÍCH NGHIÊN CỨU KHOA HỌC Đoạt giải khuyến khích phong trào sinh viên nghiên cứu khoa học cấp trường với đề tài “Thiết Kế Chế Tạo Hệ Cánh Tay Máy Bậc Tự Do“ - 118 - ... THIẾT KẾ BỘ ĐIỀU KHIỂN 4.1 Giới thiệu: Nội dung chương tác giả trình bày lý thuyết điều khiển Lật Ngược điều khiển Cân Bằng lắc Bộ điều khiển cân Chuyển điều khiển Bộ điều khiển lật ngược Hình... TÀI: XÂY DỰNG BỘ ĐIỀU KHIỂN NHÚNG CHO CON LẮC NGƯỢC QUAY 2- NHIỆM VỤ LUẬN VĂN: • Xây dựng phần khí điện tử mơ hình lắc ngược quay • Thu thập liệu từ mơ hình thực để ước lượng tham số mơ hình lắc. .. số lắc phương pháp Bình Phương Cực Tiểu sau xây dựng luật điều khiển dựng phương pháp K Futara - M Yamakita đề xuất giữ lắc cân bằng phương pháp Điều Khiển Đặt Cực, LQR Luật điều khiển xây dựng