Nghiên cứu thử nghiệm một hệ điều khiển tự cân bằng trên cơ sở sử dụng cảm biến gia tốc và cảm biến vận tốc góc : Luận văn ThS. Kỹ thuật điện tử - viễn thông: 2 07 00

ĐẠI HỢC QC GIA HÀ NỘI TRƯỜNG ĐẠI HỌC CƠNG NGHỆ Phạm Duy H n g N G H I Ê N T Ụ C Â N G I A Ngành: C Ú Ư T H Ủ B Ằ N G T Ố C N G H I Ệ• M T R Ê N V À c o C Ả M S Ỏ B I É N M Ộ• T s H Ệ• Đ I È U D Ụ N G V Ậ N T Ó C K H I Ẻ N C Ả M B I Ế N G Ĩ C Điện tử-Viễn thơng Chun ngành: Kỳ thuật vô tuyến điện tử thông tin liên lạc Mã số: 2.07.00 L U Ậ N V Ã N T H Ạ C SỸ NGƯỜI HƯỚNG DÀN KI IOA HỌC TS T r ầ n Q u a n g Vinh HÀ NỘI - 2006 M ỤC LỤC LỜI CẢM Ơ N LỜI CAM ĐOAN DANH MỤC HÌNH VẼ, ĐỊ THỊ MỞ ĐÂU Chương - LÝ THUYẾT HỆ ĐIÊU KHIẾN T ự CÂN BẲNG 10 L I M Ơ H ÌN H H Ệ C O N L Ắ C N G Ư Ợ C 11 1.1.1 Xây dựng phương trình tốn 11 1.1.2 Thiết kế mơ hình khơng gian trạng thái cho Hệ lắc ngược 14 1.2 M Ỏ H ÌN H X E T ự C Â N BẦNG TRÊN H A I BẢNH.' IX 1.3 TÍNH ĐIÊU K H IÊ N Đ ợ c CỦA H Ệ T H Ô N G 22 Chương - CẢM BIẾN VI c ĐIỆN TỪ 24 2.1 C Ả M BI É N GIA TÔC A D X L 2 : 24 2.1.1 Nguyên tắc đo gia tốc: 24 2.1.2 Đặc trưng cùa cảm biến ADXL202 26 2.1.3 Một sổ khảo sát ADXL202 .27 2.2 C O N Q U A Y VI c o A D X R S I5 30 2.2.1 Đặc trưng cùa Gyroscope ADXRS150 31 2.2.2 Một sổ khảo sát cảm biến ADXRS150 34 Chương - x LÝ TÍN HIỆU TỪ CẢM BIẾN 37 ỉ THỈÉT K É B ộ LỌ C B Ù Đ Ể x L Ý TÍN H IỆU G Ó C N G HIÊNG 37 3.1.1 Thiết kế lọc với cảm biến lý tưởng 39 3.1.2 Thiết kế lọc với động lực học cảm biến gia tốc: 40 B ộ LỌ C THÍCH N G H I K A L M A N 43 3.2.1 Bản chất tính tốn lọc: 43 3.2.2 Bàn chất thống kê lọc 45 3.2.3 Khảo sát thuật tốn Kalman thơng qua ngơnngừ Matlab 47 3.2.4 Thuật tốn Kalinan xử lý tín hiệu góc nghiêng 50 3.3 ĐẢNH GIẢ Bộ LỌC BÙ VÀ Bộ LỌC KALMAN 52 Chương - MÔ PHỎNG VÀ THỪ NGHIỆM MƠ HÌNH XE HAI BÁNH T ự CÂN BẲNG 53 ỉ GIỚI THIỆU PHÀN MÉM MÒ PHỎNG 3D: V1SUALNASTRAN DESKTOP 53 4.2 THIẾT KÉ MƠ HÌNH XE HAI BẢNH Tự CẦN BẰNG 55 4.2.1 Các đối tượng cùa mơ hình: 55 4.2.2 Thiết lập thuộc tính đối tượng 56 4.2.3 Thêm tham số điều khiển đo lường 57 4.2.4 Truyền tham số VisualNastran MatLab 57 THỬ NGHIỆM XÂY DỰNG XE Tự CÂN BẰNG TRÊN HAI BÁNH 59 4.3.1 Kết cấu khí xe 59 4.3.2 Thực thi phần cứng phần mềm điều khiển 60 KẾT LUẬN 64 DANH MỤC CÁC CƠNG TRÌNH CỦA TÁC GIÀ ĐẢ CỔNG B ố 65 TÀI LIỆU THAM KHẢO .66 PHỤ LỤC 69 Phụ lục 1: Chương trình điều khiển viết chip PSoC 69 Phụ lục 2: Chương trình Matlab khảo sát thuật toán điều khiển LQR 76 Phụ lục 3: Chương trình Matlab mơ thuật tốn Kalman .78 Phụ lục 4: Chương trình thị góc nghiêng roll-pitch máy tính 80 DANH M Ụ C H ÌN H VẼ, Đ Ị T H Ị Hình Mơ hình xe hai bánh tự cân Hình Mơ hình lắc ngược gẳn đếchuyển động haibánh Hình Các lực tác dụng lên hệ lẳc ngược Hình Sơ đồ điều khiển phản hồi vịng kín Hình Đồ thị biểu diễn quan hệ vị trí củaxe,góc nghiên lắc theo thời gian tác dụng xung nhẩy bậc lối vào Hình Đáp ứng điều khiển LQR với giá trị khởi đầu lớn Hình Sơ đồ điều khiển phàn hồi với lối vào tham chiếu Hình Đáp ứng thời gian với lối vào tham chiếu Hình Sụ tương quan đáp ứng hệ vật lý với đồ thị Hình 10 Phân tích lực mơmen cùa mơ hình xe tự cân hai bánh Hình 11 Sơ đồ thực chuyển đổi hai loại mơmen xoắn Hình 12 Hệ khối lượng - lò xo sử dụng để đo gia tốc Hình 13 Mơ hình tụ điện đơn (bên trái) hai tụ nối tiếp (bên phải) Hình 14 Cấu trúc cảm biến gia tốc ADXL202 Hình 15 Hình chiếu cùa véctơ gia tổc trọng trường lên hai trục cảm biến gia tổc sử dụng đổ đo góc nghiêng Hình 16 Tín hiệu độ rộng xung lối số cảm biến gia tốc Hình 17 Đồ thị biểu diễn phụ thuộc độ rộng xung góc nghiêng Hình 18 Mơđun đo góc nghiêng cùa vật thể chuyển động: a) Môđun đo gắn vật thể chuyển động b) Giao diện đồ hoạ phần mềm chi thị góc nghiêng roll pitch Hình 19 Hệ điều khiển thực ảo cho tay máy bậc tự sử dụng cảm biến ADXL202: a) Mơ hình tốn điều khiển thực ảo b) G iao diện đồ hoạ cùa phần mềm chi thị trạm điều khiển c) Ảnh chụp người điều khiển lệnh cho Robot bàng tay (cảm biển gắn tay người điều khiển) Mình 20 Ảnh chụp cảm biến A D X R S 150 Iỉình 21 Tín hiệu lối R A TEO U T tăng theo chiều quay thuận chiều kim đồng hồ Iỉình 22 Cấu trúc A D X R S150 Hình 23 Sơ đồ đấu nối A D X R S 150 với linh kiện phụ Hình 24 Sơ đồ khối đo vận tốc góc sử dụng cảm bién A D X R S 150 H ình 25 đồ thị quan hệ vận tốc góc giá trị số thu từ biến đổi ADC trường hợp: quay theo chiều thuận (hình trái); quay theo chiều ngược(hình phải) Iỉình 26 a) Bàn xoay có gắn vạch đen trắng b) Sơ đồ khối Môđun đo vận tốc góc cùa bàn xoay Hình 27 Ý tưởng thiết kế lọc bù Hình 28 Sơ đồ khối lọc bù Hình 29 Giản đồ bode cùa hệ thống ước lượng với số thời gian khác Hình 30 Đáp ứng tần số đáp ứng pha lọc thơng ihap nhánh có cảm biến g óc nghiêng Hình Đáp ứng tần số đáp ứng pha cùa lọc thơng cao nhánh có cảm biến vận tốc góc Minh 32 Tổ hợp hai nhánh tới lối lọc Đường liên tục lối lọc dường đứt nét tín hiệu nhánh Hình 33 Thuật tốn lọc Kalman Hình 34 Sơ đồ thực thuật tốn Kalman Hình 35 Bộ lọc Kalman Hình 36 Sơ dồ khối hệ thống khảo sát ví dụ Ilình 37 Ket q thực chương trình(m file) Matlab khảo sát thuật tốn Kalman 1Hnh 38 Giao diện chương trình VisualNastran 4D Hình 39 M hình xe hai bánh tự cân Hình 40 Cửa sổ thuộc tính cùa đối tượng w h e e lje f t Hình 41 Khối vNPlant thực truyền tham số VisualNastran MatLab Hình 42 Sơ đồ thực thi thuật thuật toán điều khiển xe hai bánh tự cân thiết Hình 43 Đ thị quan hệ góc nghiêng, vận tốc góc xe hai bánh theo thời gian Hình 45 Sơ dồ khối điều khiển xe tự cân hai bánh Hình 46 Thiết lập tài nguyên người dùng bên chip PSoC Hỉnh 47 Sơ đồ nguyên lý mạch điều khiển xe cân hai bánh MỞ ĐẦU Trong năm gần đây, việc ứng dụng hệ thống vi điện từ M EM S (M icro Electro M echanical System ) tạo thay dổi lớn công nghệ chế tạo cảm biến quán tính (cảm biến gia tốc, vận tốc g ó c , ) Mặc dù, cảm biến chưa đạt độ xác cảm biến học cổ điển có ưu điểm hom hẳn mặt kính thước, cơng suất, giá thành C ùng với xu hướng đó, nhiều phịng thí nghiệm giới triển khai nghiên cứu ứng dụng cảm biến việc xây dựng chế tạo hệ điều khiển tự cân bằng, khởi đầu cho đời cùa robot dạng người (humanoid robot), lĩnh vực hứa hẹn nhiều thành công khả úng dụng rộng rãi tương lai Tại Khoa Cơ khí, trường Đại học Bách khoa Thành phố Hồ Chí Minh, nghiên cứu chế tạo xe hai tự cân trờ người (m ô hình Segw ay) thực thu kết tốt [3] Nhàm mục đích nghiên cứu bước đầu thừ nghiệm lý thuyết điều khiển cân áp dụng cho m hình xe tự cân hai bánh kích thước nhỏ sử dụng cảm biến M EM S, Bản luận văn tập trung làm sáng tỏ vấn đề nội dung dược trình bày chương sau: C h o n g 1: N ghiên cứu lý thuyết điều khiển cân bàng lắc ngược m ô hình xe tự cân hai bánh xe C h o-ng 2: N ghiên cứu, triển khai cảm biến gia tốc vận tổc góc hãng A n alog D evice ứng dụng đo góc nghiêng C h u o n g 3: N ghiên cứu phương pháp xử lý tín hiệu góc nghiêng kết hợp hai loại cảm biến gia tốc vận tốc góc C h n g 4: Thực mô thử nghiệm xây dựng hệ điều khiển tự cân hai bánh xe V ới kết đạt việc thừ nghiệm xây dựng hệ xe cân bang hai bánh sừ dụng cảm biến gia tốc cảm biến vận tốc góc chế tạo theo g nghệ M EM S, tác già hy vọng tiếp tục phát triển đề tài tiến dần đến việc chế tạo robot tự giữ thăng hai chân có xử lý mạnh hơn, đủ đáp ứng thực thuật tốn địi hịi độ phức tạp cao Vi thời gian khả có hạn, luận văn khơng tránh khỏi thiếu sót Tác giả m ong nhận góp ý thầy cô giáo, bè bạn, đồng nghiệp quan tâm tới đề tài Chương LÝ THUYÉT HỆ ĐIÈU KHIẺN TỤ CÂN BẦNG Hứa hẹn nhiều ứng dụng rộng rãi tương lai, Hệ thống điều khiển tự cân bang từ đon giản (con lắc ngược ) đến phức tạp (hệ thống xe hai bánh tự cân bằng, robot hai chân, ) nghiên cứu, triển khai nhiều phịng thí nghiệm giới [19, 20, 21] V ới mục đích nghiên cứu, thừ nghiệm hệ điều khiển tự cân bằng, chương đề cập lý thuyết điều khiển m hình xe hai bánh tự cân hình với chất lắc ngược Hình I Mơ hình xe hai bánh tự cân Đ ây mô hình gồm bậc tự - X e quay quanh trục z (pitch) tạo góc nghiêng 0p so với phương thẳng đứng tương ứng với vận tốc góc cop - Chuyển động thẳng theo trục X r m với vận tốc góc V RM - Quay quanh trục thẳng đứng (yaw ) với góc V iệc xây dựng phương trình tốn tham sổ điều khiển liên quan làm sang tỏ sau 10 1.1 M Ơ H ÌN H H Ệ C O N L Ắ C N G Ư Ợ C Xét mơ hình lắc ngược [5] chi hình gồm lắc có chiều dài 1, khối lượng m, đầu gắn với trục quay đế xe khối lượng M Đế chuyển động mặt sàn thơng qua hệ thống bánh xe -► X H ình M ô hình lắc ngược đư ợc gan trẽn đ ể chuyên độ n g đư ợc bằn g h bánh Các tham số cùa hệ chi sau: - M : khố i lượng - m: khối lượng cùa lắc - 1: chiếu củ a xe dài cùa lắc số ma - b: - ỉ: - F: lực tác dụng vào xe - x: vị - : góc hệ sát cù a xe đổi với mặt sàn hệ số quán tính lắc trí cùa xe hệ toạ độ lắc so với phương thẳng đứng 1.1.1 X â y d ự n g p h n g trình tốn Hình chi lực tác dụng lên m hình lắc ngược theo hai phần riêng biệt: - Tác dụng lên lắc - Tác dụng lên thân xe 11 P W M _l_S ta rt(); } PRTOGS &= 0x7 F ; //d isco n n e ct PRTODR &= 0x7 F ; / / P0.7 = P0.7 } v o id M l_S to p (){ P W M _l_Sto p {); PRTOGS &= 0x7F //d isco n n e ct P0.7 PRT2GS &= 0x7F //d isco n n e ct P PRTODR 1= 0x80 //set PRT2DR |= 0x80 // P0.7 & P2.7 } ///////////////////////////////////////////////////////////// v o id C o n tro l_P o sitio n _M (ch a r check, in t if(c h e c k ) M _ F o r w a r d (V E L ) ; e lse M 2_Backw ard(V EL); V E L ,in t i) { P o sitio n _M _2 _S to p (); P o sitio n _M _2 _W rite P e rio d (i ); P o sitio n _M _2 _S ta rt(); } vo id M 2_Forw ard(char x ) { PW M 8_2_Stop() ; if(x > ){ PRTOGS &= OxDF; //d isco n n e ct PRTODR 1= 0x20; //set 1= 0x20; P P } e lse { PRTOGS //connect P W M _2 _W rite P u lseW id th (x ); PW M 8_2_Start(); 72 P } PRT2GS &= OxDF ; //d isco n n e ct PRT2DR &= OxDF; //P2.5 = P } v o id M 2_Backw ard(char x) { PW M 8_2_Stop 90) { PRT2GS &= OxDF; //d isco n n e ct PRT2DR |= 0x2 0; //set 1= 0x2 0; //connect P P } e lse { PRT2GS P P W M _2 _W rite P u lseW id th (x ) ; PW M 8_2_Start(); } PRTOGS &= OxDF; //d isco n n e ct PRTODR &= OxDF; / / P0 = P0.5 } v o id M 2_Stop(){ PW M 8_2_Stop(); PRTOGS &= OxDF; //d isco n n e ct P PRT2GS &= OxDF; //d isco n n e ct P PRTODR 1= 0x2 0; //set PRT2DR |= 0x2 0; // P0.5 & P2.5 } v o id m a i n () { //e n a b le M C _E n a b le G In t; P o sitio n _M _l_E n a b le In t(); P o sitio n _M _2 _E n a b le In t(); w h ile d ) { 73 g lo b al for(i=0;i 5190) VEL = e lse tic k 20+(4*(5715 VEL = = - countl))/55; 98; 1; C o n tro l_P o sitio n _M l(B A C K W A R D ,V E L ,2 0 0 ); C o n t r o l _ P o s i t i o n _ M (B A C K W A R D ,V EL,2 0 0 ) ; } e lse if(c o u n tl > 5715){ i f (c o u n t1 < )V E L = + ( * ( c o u n t l e lse VEL tic k = = 0; C o n t r o l _ P o s i t i o n _ M l ( FO R W A R D ,V EL,2 0 0 ) ; C o n t r o l _ P o s i t i o n _ M (FO R W A R D ,V EL, 0 0 ) ; } e lse { i f ( t i c k ) { C o n t r o l _ P o s i t i o n _ M l (BA CKW ARD ,VEL, 0 0 ) ; C o n tro l_P o sitio n _M (B A C K W A R D ,V E L,2 0 0 ); } e lse { Control_Position_Ml(FORWARD,VEL,20000); C ontrol_P osition_M 2(FORWARD,VEL,2 0000) ; } } } M 2_Forw ard(100); M l_B ack w ard (100); 74 98; 5715))/55; #pragma in terru p t_h an d ler Position_M_l_ISR_C void Position_M _l_ISR_C(){ Ml_Stop(); } #pragm a v o id in te rru p t_h a n d le r P o sitio n _M _2 _IS R _C P o sitio n _M _2 _IS R _C (){ M 2_Stop(); } 75 Phụ lục 2: Chương trình Matlab khảo sát thuật tốn điều khicn LQR %llllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllll% C huong trinh mo phong khao sat thuat toan LQR V ollllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllllll m = 0.5; m = 0.2; b = 0.1; i = 0.006; g = 9.8; = 0.3; p = i*(M + m )+ M * m * lA2; A = [0 I 0 -(i+m *lA2)*b/p (m A2*g*lA2)/p 0 0 -(m*l*b)/p m *g*l*(M +m )/p 0; 1; 0]; B = [0; (i+m *lA2)/p; 0; m*l/p]; c =[1000; 00 10]; D = [0;0]; x=5000; theta=200; Q=[x 0 0; 0 0; 0 0; 0 theta 0]; R = I; K = lq r(A ,B ,Ọ ,R ) Ac = [(A-B*K)]; 76 Be = [B]; Ce = [CJ; De = [D]; T = : 0 1:5; U =0.2*ones(size(T)); [Y ,X ]=lsim (A c,B c,C c,D c,U ,T); plot(T,Y ) legend('C art','Pendulum') 77 P h ụ lục 3: ChưoTig t r ì n h M a t l a b mô p h ỏ n g t h u ậ t t o n K a l m a n V o lllllllllllllH IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII %/// Chuông trinh mo phong thuat toan Kalman % IIIIIIIIIIIIIH IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIH IIIIIIIIIII A=[ 1.1269 -0.4940 0.1129 1.000 0 I 0]; B=[-0.3832 0.5919 0.5191]; c=[l 0]; plant=ss(A, [B 13], c, ,-1, 'ỉnputname', {'u' V } , 'Outputname','y'); % phuong trinh trang thai he plant 0=1; R=1; [kalm f,L,P,M ]=kalm an(plant,Q,R); kalm f=kalm f(l,:); a = A; b = [B B 0*B]; c = [C;C]; d = [0 0;0 1]; p = ss(a,b,c,d,-l,'m putnam e\{'u ' 'w' V } ,'o u u tn a m e ',{ y 'yv'}); sys=paralleI(P,kalmf, 1,1,[],[]); SimM odel = fe edb ack (sy s,l,4,2,l); SimM odel = Sim M odel([l 3],[1 3]); t = [0:100]*; u = sin(ư5); n = length(t); randn('seed',0); 78 w = sqrt(Q )*randn(n,l); v = sqrt(R )*randn(n,l); [out.x] = lsim(SimM odel,[w,v,u]); y = out(:, 1); % true response ye = out(:,2); % filtered response yv = y + v; % m easured response subplot(2l I), plot(t,y,' ',t,ye,'-'),legend('Loi ly tuongVLoi duoc loc'); xlabel('So m au(No o f Sample)'), ylabel(’Loi ra(output)') title(’Dap ung cua mach loc (Kalman filter response)') s ubplot(2I2), plot(t,y-yv,'-.',t,y-ye,'-'),legend('Loi do','Loi loc'); xlabel('So m au(N o o f Sample)'), ylabel('Loi(error)') 79 P h ụ lục 4: C h n g t r ì n h thị góc n g h i ê n g roll-pitch t r c n m y tính Dim Roll As Double Dim Pitch As Double Dim degS Dim pi, x l , y l , x2, y2, temp, DI As Double Dim a l , A2, a3, a4, A As Double Dim Pmax, Pmin, n u m l, num2 As Double Dim P2max, P2min A s Double Dim init As Integer Privatc Sub C o m m a n d l_ C lick () End End Sub Private Sub D raw _ ai\(P itch As Double, Roll As Double) Dim j As Integer DI = 0 + 1260 * Sin(Pitch) Line4(0).yl = D Line4(0).y2 = DI L in e4 (l).y l = DI - 1260 L ine4(l).y2 = DI - 1260 Line4(2).yl = D + 1260 Line4(2).y2 = DI + 1260 Line4(3).yl = DI - 60 Line4(3).y2 = DI - Line4(4).yl = DI + Line4(4).y2 = DI + Line4(5).yl = DI - 180 Line4(5).y2 = DI - 180 Line4(6).yl = DI + ] 80 80 L¡ne4(6).y2 = D l + 180 Line4(7).yl = D l - Line4(7).y2 = D l - Line4(9).yl = D l + 420 Line4(9).y2 = D l + 420 Line4( 10).y = DI - Line4(IO).y2 = D l - L in e4 (ll).y l = Dl + Linc4(l l).y2 = D l + D w lin e(l).x l = 2460 - 5480 * Cos(Roll) D w lin e(l).y l = 2100 - Sin(Roll) * 5480 D raw line(]).x2 = 2460 + 5480 * Cos(Roll) D raw line( 1).y2 = 2100 + Sin(Roll) * 5480 End Sub Private Sub cm dR eset_C lick() Dirn i As Integer D w lin e(l).x l = xl D raw line(l).x = x2 Drawl in e(l).y l = yl D raw line(l).y = y Roll = Pitch = For i = To scrIRotate.Count - scrlRotate(i) Value = lblRotate(i).Caption = "0.00" & degS Next i Line4(0).yl = 0 Linc4(0).y2 = 2100 81 L m e4 (l).y ] = 0 - Line4(l).y2 = 2100 - 1260 Linc4(2).y = 0 + Linc4(2).y2 = 2100 + 1260 Line4(3).yl = 0 - Line4(3).y2 = 0 - Line4(4).yl = 0 + 60 Linc4(4).y2 = 2100 + 60 Line4(5).yl = 0 - Line4(5).y2 = 0 - 180 Line4(6).yl = 0 + Line4(6).y2 = 2100 + 180 Line4(7).yi = 0 - Line4(7).y2 = 0 - Line4(9).yl = 0 + 420 U ne4(9).y2 = 0 + 420 L in e4(I0).y l = 0 - Line4(10).y2 = 2100 - L i n e ( l l ) y l = 0 + 780 Line4( 11 ).y2 = 100 + 780 End Sub Private Sub C om m and2_Click() Dim si As Double I f Not Is N u m e ric(T e x tl(l).T ex t) Then M sgB ox "Please enter numbers" Exit Sub End If s i = pi * C D b l(T e x tI(l).T e x t) / 180 82 D r a w a i x Pitch, si Roll = si T e x t ! ( l ) T e x t = "" End Sub Private Sub C om m and4_Click() Dim s As Double If Not IsN um eric(T extl(0).T ext) Then M sgB ox "Please enter numbers" Exit Sub End If s —pi * C D b l(T extl(0).T ex t) / 180 Draw_aix s, Roll Pitch = s T ex tl(0 ).T e x t = "" End Sub Private Sub Form_Load() Form I.Caption = "Plane Simulation" pi = 3.14159265358979 degS = Chr(176) xl =-1020 x2 = 5940 yl = 0 y2 = 2100 tem p - Pitch = Roll = s= si = 83 cm dR esetC lick Init com port M S C om m 4.C o m m Port = M S C om m 4.Settings = "9600,N ,8 , 1" M SC om m 4.lnputLen = I M S C om m 4.PortO pen = True T im erl.E n ab led = False End Sub Private Sub Form _Unload(Cancel As Integer) M S C om m 4.P ortO pen = False End Sub Function get_values() As Double Dim Instring As String Do DoEvents Loop Until M S C om m 4.lnB ufferC ount o Instring = M SCom m Input g e t v a l u e s = Val(lnstring) End Function Private Sub M S C om m 4_O nC om m () Pmin = 3466 Pmax = 5711 P2max = 5744 P2min = 3499 A = (P m ax - Pmin) / A2 = (P2max - P2min) / If (init) Then num = get_values() 84 If (init = 2) Then If (num > P2max) Then num = P2max If (num < P2min) Then num = P2min Pitch = vbArcSinus((num - (A2 + P2min)) / A2) '+ * pi / 180 init = M S C om m 4.R T hreshold = T ext3.Text = -Round(Pitch * / p i ) End If If (¡nit = 1) Then I f (nurn > Pmax) Then num = Pmax If (num < Pmin) Then num = Pmin Roll = (vbArcSinus((num - (A + Pmin)) / A)) + 1.3 * pi / 180 T ext4.Text = -Round(Roll * 180 / pi) ¡nit = End If D r a w a i x Pitch, Roll Else If (M S C om m Input = "!") Then init = M S C om m 4.R Threshold = M S C om m 4.InputL en = End If End If End Sub Private Sub Reset_Click() cm dR esetC lick End Sub Private Sub scrlR otate_S croll(Inde\ As Integer) 85 Dim rad As Double, deg As Double rad = pi * scrlRotate(Index) / 180 dcg = scrlRotate( Index) Select C ase Index Case 0: D raw aix rad Roll Pitch = rad C ase 1: D r a w a i x Pitch, rad Roll = rad End Select lblRotate(Indcx).Caption = Format(deg, "0.00" & degS) find Sub Function vbArcSinus(ByVal X As Double) As Double Inverse Sine If A b s ( X ) = Then vbArcSinus = A tn (l) * Sgn(X) * Else vbA rcSinus = Atn(X / Sqr(-X * X + 1)) End If End Function Function vbA rcC osinus(B yV al X As Double) As Double Inverse Cosine If Abs(X ) = Then vbA rcC osinus = Atn( 1) * ( - Sgn(X)) * Else vbA rcC osinus = Atn(-X / Sqr(-X * X + 1)) + * A tn (l) End If End Function 86