VISION BASED TARGET TRACKING GUN TURRET A dissertation submitted to the Department of Electrical Engineering, University of Moratuwa in partial fulfillment of the requirements for the Degree of Master of Science by WANNIACHCHIGE INDU DILSHAN ABEYW ARDENA Supervised by: Dr. Rohan Munasinghe Department of Electrical Engineering University of Moratuwa Sri Lanka 2010 94544 Abstract This project explores the task of tracking a moving target (aircraft) and pointing an anti aircraft gun perched on a pan tilt base in a real time combat environment. The purpose of this process is to study the dynamics of the mechanism, controlling requirements, software requirements and subsystems requirements needed for the implementation of a commercially viable air defense system. A laboratory model is developed to represent the pan-tilt gun turret, dynamically analyzed, controllers designed and field testing carried out. Though the system is designed as a laboratory model, every attempt is made to reach the level of sophistication and detail required for a military grade target tracking system. The thesis formulates a clearly identifiable procedures and steps which need to be carried out in the implementation of such a system. Due to the obvious unavailability of target tracking data from a radar system, target position information from machine vision software is used. The scope of the project is limited to; designing the interface between Camera, CPU and servo-controllers, designing the mounting base for the artillery with the pan-tilt mechanism, Modeling the plant and designing the ,controller. Field testing reveals the validity of the procedures mentioned above and the satisfactory results obtained through such procedure. D E C L A R A T I O N T h e w o r k s u b m i t t e d i n t h i s d i s s e r t a t i o n i s t h e r e s u l t o f m y o w n i n v e s t i g a t i o n , e x c e p t w h e r e o t h e r w i s e s t a t e d . I t h a s n o t a l r e a d y b e e n a c c e p t e d f o r a n y d e g r e € , a n d i s a l s o n o t b e i n g c o n c u r r e n t l y s u b m i t t e d f o r a n y o t h e r d e g r e e . I " l / I '-.-~-- D i l s h a n . A b e y w a r d e n a 0 8 / 0 2 / 2 0 1 0 I e n d o r s e t h e d e c l a r a t i o n b y t h e c a n d i d a t e . .~ I . l 1 I I I ' ' 1 ' 1 J ! i I / u " - / / i { ' v ~-- · - · - - - - ; D r R o h a n M u r i a s i n g h e lia~~- D e c l a r a t i o n A b s t r a c t A c k n o w l e d g e m e n t L i s t o f F i g u r e s L i s t o f T a b l e s 1 . I n t r o d u c t i o n C O N T E N T S 1 . 1 T a r g e t T r a c k i n g F u n d a m e n t a l s 1 . 2 M o t i v a t i o n 1 . 3 G o a l s a n d A c h i e v e m e n t s 2 . T a r g e t I d e n t i f i c a t i o n a n d T r a c k i n g T e c h n i q u e s 2 0 1 S y s t e m O v e r v i e w 2 . 2 C o n t r o l T e c h n o l o g i e s 2 . 3 L i n e a r S e c o n d O r d e r S I S O M o d e l 2 . 4 M o t o r M o d e l - ' 3 . I m p l e m e n t a t i o n o f V i s i o n B a s e d T a r g e t T r a c k i n g S y s t e m 3 . 1 D e s i g n a n d C o n s t r u c t i o n o f t h e T u r r e t B a s e 3 . 1 0 1 R o t a t i o n B a s e 3 . 1 . 2 T i l t B a s e 3 . 1 . 3 M o u n t i n g P l a t e 3 . 1 . 4 T i l t M o v e m e n t G e a r B o x 3 . 1 . 5 C o m p l e t e S y s t e m 3 . 1 . 6 D e s i g n S p e c i f i c a t i o n s 3 . 1 . 7 S e l e c t i o n o f M o t o r s 3 0 1 . 8 S e l e c t i o n o f S e n s o r s 3 . 1 . 9 S y s t e m I n e r t i a C a l c u l a t i o n 3 . 2 V i s i o n S y s t e m 3 . 2 . 1 P i p e P r o g r a m 4 . C o n t r o l l e r D e s i g n 4 . 1 B a n d w i d t h C a l c u l a t i o n 4 . 1 . 1 R e q u i r e d B a n d w i d t h 4 . 1 . 2 A v a i l a b l e B a n d w i d t h 4 . 2 D e s i g n o f E l e c t r o n i c s 4 . 3 L o o p S h a p i n g a n d T r a c k i n g R e s p o n s e 4 . 4 S t a b i l i t y o f t h e S y s t e m 4 . 5 S y s t e m w i t h P o s i t i o n F e e d b a c k 4 . 6 D i g i t a l C o n t r o l l e r 4 . 7 T r a j e c t o r y P l a n n i n g I V v V I V l l 1 1 2 3 4 5 5 7 1 0 1 1 1 1 1 2 1 3 1 3 1 4 1 4 1 5 1 5 1 6 1 8 1 9 2 0 2 1 2 1 2 1 2 7 3 2 4 0 4 3 4 3 4 5 4 8 1 1 . . 5 . R e s u l t s 5 . 1 R e s p o n s e t o E x t e r n a l T a r g e t I n p u t s 5 . 2 R e s p o n s e t o S i n u s o i d a l I n p u t s 6 . C o n c l u s i o n s a n d F u r t h e r D e v e l o p m e n t s R e f e r e n c e s A p p e n d i x A C o - o r d i n a t e T r a n s f o r m a t i o n u s i n g I n v e r s e K i n e m a t i c s A p p e n d i x B P i c B a s i c P r o S o u r c e C o d e A p p e n d i x C M a t l a b f i l e f o r F F T A n a l y s i s - ' A p p e n d i x D M a t l a b f i l e f o r T F E x t r a c t i o n f r o m S i m u l i n k M o d e l A p p e n d i x E V B S c r i p P r o g r a m f o r R o b o r e a l m A p p e n d i x F Q u a d r a t u r e E n c o d e r A p p e n d i x G S i m p l e P o t e n t i o m e t e r A p p e n d i x H A b s o l u t e E n c o d e r C o n t a c t L e s s t y p e A p p e n d i x I A b s o l u t e E n c o d e r C o n t a c t t y p e 5 0 5 0 5 1 5 4 5 6 5 7 6 0 6 4 6 6 6 7 6 9 7 0 7 0 7 1 1 1 1 A c k n o w l e d g e m e n t T h a n k s a r e d u e f i r s t t o m y s u p e r v i s o r , D r R o h a n M u n a s i n g h e , f o r h i s g r e a t i n s i g h t s , p e r s p e c t i v e s a n d g u i d a n c e . M y s i n c e r e t h a n k s g o t o t h e o f f i c e r s i n t h e P o s t G r a d u a t e O f f i c e , F a c u l t y o f E n g i n e e r i n g , U n i v e r s i t y o f M o r a t u w a , S r i L a n k a f o r h e l p i n g i n v a r i o u s w a y s t o c l a r i f y t h e t h i n g s r e l a t e d t o m y a c a d e m i c w o r k s i n t i m e w i t h e x c e l l e n t c o o p e r a t i o n a n d g u i d a n c e . S i n c e r e g r a t i t u d e i s a l s o e x t e n d e d t o t h e o f f i c e r s w h o s e r v e i n t h e D e p a r t m e n t o f E l e c t r i c a l E n g i n e e r i n g o f f i c e . T h e a u t h o r e x t e n d s s i n c e r e g r a t i t u d e t o P r o f e s s o r L a n k a U d a w a t t a a n d D r . C h a n d i m a D e d d u w a P a t h i r a n a f o r t h e i r u n t i r i n g e f f o r t s i n c o o r d i n a t i n g t h e M S c i n I n d u s t r i a l a u t o m a t i o n p r o g r a m a n d e n c o u r a g i n g w o r d s i n e v e r y s t e p o f t h e w a y o f t h e a b o v e p r o g r a m . T h e a u t h o r h a d t o m a k e m a n y v i s i t s t o t h e l o c a l m a c h i n i s t i n t h e p r o c e s s o f d e s i g n i n g a n d b u i l d i n g t h e p a n - t i l t b a s e a n d w a s p l e a s a n t l y s u r p r i s e d b y t h e s k i l l a n d a p t i t u d e o f t h e s e e x p e r i e n c e d c r a f t m e n . T h e d e s i g n p r o c e s s w a s a m a r r i a g e b e t w e e n m e c h a n i c a l e n g i n e e r i n g a n d e l e c t r o n i c s e n g i n e e r i n g a m o n g o t h e r d i s c i p l i n e s , T o t h i s e n d , I m a y n o t h a v e b e e n a b l e t o f i n i s h o f f t h e m e c h a n i s m i f n o t f o r v a l u a b l e s u g g e s t i o n s f r o m w o r k e r s a t t h e l o c a l m a c h i n i s t s h o p . L a s t l y , I s h o u l d t h a n k m y f a m i l y f o r t h e i r u n d e r s t a n d i n g a n d p e r s e v e r a n c e d u r i n g t h e l a s t t w o y e a r s , f r i e n d s a n d c o l l e a g u e s w h o h a v e n o t b e e n m e n t i o n e d h e r e p e r s o n a l l y i n m a k i n g t h i s e d u c a t i o n a l p r o c e s s a s u c c e s s . I c o u l d n o t h a v e m a d e i t w i t h o u t y o u r s u p p o r t s . v L i s t o f F i g u r e s F i g u r e P a g e 2 . 1 A u t o m a t e d t a r g e t t r a c k i n g a i r d e f e n s e s y s t e m 5 2 . 2 S y s t e m m o d e l 6 2 . 3 L i n e a r s e c o n d o r d e r S I S O m o d e l o f a m a n i p u l a t o r j o i n t 7 2 . 4 M o t o r m o d e l 1 0 3 . 1 S o l i d w o r k s i m p r e s s i o n o f t h e f i n a l g u n t u r r e t s y s t e m 1 1 3 . 2 R o t a t i o n b a s e 1 2 3 . 3 F i x i n g p l a t e f o r s e n s o r p o t e n t i o m e t e r . : 1 2 3 . 4 T i l t b a s e 1 3 3 . 5 M o u n t i n g p l a t e 1 3 3 . 6 T i l t m o v e m e n t g e a r b o x 1 4 3 . 7 C o m p l e t e s y s t e m 1 4 3 . 8 C a l c u l a t i n g m o m e n t o f i n e r t i a a r o u n d y a x i s 1 8 3 . 9 R o b o r e a l m s t a r t u p s c r e e n 1 9 3 . 1 0 F l o w d i a g r a m o f p i p e p r o g r a m 2 0 4 . 1 V e l o c i t y p r o f i l e o f a n A - 1 0 t h u n d e r b o l t a t m a x i m u m a c c e l e r a t i o n 2 2 4 . 2 M o v e m e n t ( d i s t a n c e ) p r o f i l e o f a n A - 1 0 t h u n d e r b o l t a t m a x i m u m a c c e l e r a t i o n 2 3 4 . 3 M o v e m e n t o b s e r v e d f r o m a d i s t a n c e o f 3 0 0 0 m 2 4 4 . 4 V a r i a t i o n o f p a n a n g l e w h e n t h e m o v e m e n t i s o b s e r v e d f r o m 3 0 0 0 m 2 4 4 . 5 P o w e r s p e c t r u m o f t h e t a r g e t m o v e m e n t 2 5 4 . 6 V a r i a t i o n o f a n g u l a r v e l o c i t y w i t h t i m e 2 6 4 . 7 V a r i a t i o n o f a n g u l a r a c c e l e r a t i o n w i t h t i m e 2 6 4 . 8 S y s t e m r e s p o n s e t o s i n u s o i d a l i n p u t s o f i n c r e a s i n g f r e q u e n c y 2 7 - 3 1 4 . 9 G a i n v s f r e q p l o t f o r p a n m o v e m e n t 3 2 4 . 1 0 P I D c o n t r o l l e r a n d p l a n t 3 2 4 . 1 1 O v e r a l l c o n t r o l a r c h i t e c t u r e o f t h e s y s t e m 3 3 4 . 1 2 P I C 1 6 f 8 7 7 m i c r o c o n t r o l l e r s u p p o r t P C B 3 4 4 . 1 3 P I C 1 6 f 8 7 7 m i c r o c o n t r o l l e r P C B ( p o p u l a t e d w i t h c o m p o n e n t s ) 3 4 4 . 1 4 S c h e m a t i c a n d P C B o f t h e H - B r i d g e 3 5 4 . 1 5 S i m u l i n k m o d e l o f t h e c o m p l e t e s y s t e m 3 8 4 . 1 6 S i m u l i n k c l o s e d l o o p m o d e l o f t h e c o m p l e t e s y s t e m 3 9 4 . 1 7 A c t u a l s t e p r e s p o n s e v s s i m u l i n k m o d e l s t e p r e s p o n s e 3 9 4 . 1 8 V e l o c i t y s t e p r e s p o n s e o f t h e s y s t e m 4 1 4 . 1 9 B o d e p l o t o f t h e s y s t e m 4 1 4 . 2 0 B o d e p l o t w i t h c a l c u l a t e d g a i n i n f o r w a r d p a t h 4 2 4 . 2 1 P h a s e m a r g i n 4 3 4 . 2 2 S y s t e m w i t h p o s i t i o n f e e d b a c k b a c k 4 3 4 . 2 3 V e l o c i t y a n d p o s i t i o n r e s p o n s e f o r s t e p i n p u t 4 4 4 . 2 4 B l o c k d i a g r a m o f P I D i m p l e m e n t a t i o n 4 8 5 . 1 R e s p o n s e t o a r b i t r a r y e x t e r n a l t a r g e t i n p u t s 5 0 5 . 2 R e s p o n s e t o s i n u s o i d a l e x t e r n a l i n p u t s 5 1 - 5 3 V l . . . L i s t o f T a b l e s T a b l e 4 J T r u t h t a b l e f o r H - B r i d g e . . . . . . . . . . . _ . : P a g e 3 5 V l l . . . . . . . . . C h a p t e r 1 I n t r o d u c t i o n 1 . 1 T a r g e t T r a c k i n g F u n d a m e n t a l s T r a c k i n g a t a r g e t i n v o l v e s t h e k n o w i n g t h e w h e r e a b o u t s o f a n o b j e c t a t a l l t i m e s . I n o r d e r t o d o t h i s , t r a c k i n g s y s t e m s m a k e u s e o f v a r i o u s f e a t u r e s o f t h e t a r g e t b e i n g t r a c k e d , s u c h a s , i t s i m a g e , r a d a r c r o s s s e c t i o n , m a s s o f t h e o b j e c t e t c . T a r g e t t r a c k i n g h a s m a n y f i e l d s o f a p p l i c a t i o n , i n t h e m i l i t a r y f i e l d t a r g e t t r a c k i n g p l a y s a m a j o r r o l e i n a u t o m a t e d m i s s i l e g u i d a n c e s y s t e m s a n d a n t i a i r c r a f t a r t i l l e r y b a t t e r i e s . I n t h e f i e l d o f i n d u s t r i a l a u t o m a t i o n , t a r g e t t r a c k i n g i s e x t e n s i v e l y u s e d i n a u t o m a t i c g u i d e d v e h i c l e s ( A G V ) . T r a c k i n g m e c h a n i s m m a y c o n t a i n s i n g l e o r m u l t i p l e s e n s o r s . I n c a s e o f m u l t i p l e s e n s o r s , d a t a f u s i o n s h o u l d b e a p p l i e d t o m a k e u s e o f m o s t s u i t a b l e d a t a f r o m e a c h s e n s o r . T h e c h a l l e n g e s t h a t n e e d t o b e o v e r c o m e d u r i n g t a r g e t t r a c k i n g a r e n u m e r o u s , t h e s i z e o f t h e t a r g e t i t s s p e e d , m a t e r i a l , c o l o r , s h a p e , b a c k g r o u n d c o n d i t i o n , t h e d i s t a n c e f r o m t h e t r a c k i n g s e n s o r , a n d t h e q u a l i t y o f t h e s e n s o r s t h e m s e l v e s i n f l u e n c e p e r f o r m a n c e . T h e t a r g e t m a y b e l a r g e a n d e a s i l y i d e n t i f i a b l e a g a i n s t a c o n t r a s t i n g b a c k g r o u n d o r i t m a y b e s m a l l a n d b l e n d i n t o t h e b a c k g r o u n d m a k i n g i t d i f f i c u l t t o d i s t i n g u i s h b e t w e e n t h e n o i s e a n d t h e a c t u a l t a r g e t E v e r y s e n s o r h a s a s e n s i n g d e l a y , I n c a s e o f p r i m a r y r a d a r s y s t e m s t h e t a r g e t i s s e n s e d o n c e e v e r y r o t a t i o n o f t h e r a d a r a n t e n n a . t h e r e f o r e b e t w e e n t w o s e n s i n g c y c l e s t h e w h e r e a b o u t s o f t h e t a r g e t i s n o t k n o w n t h e r e f o r e i f t h e t a r g e t i s a f a s t m o v i n g o n e , c o m p a r e d t o t h e s e n s o r c y c l e t i m e , d a t a g a t h e r e d f r o m t h i s s e n s o r ( r a d a r ) m a y n o t b e t o o u s e f u l i n t r a c k i n g t h e p a r t i c u l a r t a r g e t . I n c a s e o f a c a m e r a , u l t r a s o u n d s e n s o r s , i n f r a r e d s e n s o r s , t h e r m a l s e n s o r s t h e d e l a y i s d u e l a r g e l y t o t h e A t o D c o n v e r s i o n p r o c e s s . M o d e m m i l i t a r y t e c h n o l o g i e s h a v e d e v e l o p e d v a r i o u s m a t e r i a l s t h a t a b s o r b r a d a r w a v e s m a k i n g t h e r a d a r c r o s s s e c t i o n o f c e r t a i n t a r g e t s v e r y s m a l l f u r t h e r m o r e t h e t a r g e t i s s h a p e d i n s u c h a w a y t h a t i t r e f l e c t s t h e l e a s t a m o u n t o f r a d a r e n e r g y b a c k t o t h e s e n s o r s m a k i n g r a d a r d e t e c t i o n e v e n m o r e d i f f i c u l t . C o n d i t i o n ' s i n w h i c h t h e t r a c k i n g s y s t e m h a s t o o p e r a t e d o e s n o t s t a y s t a t i c . W h e t h e r c o n d i t i o n s , l i g h t i n g c o n d i t i o n s i n f l u e n c e t a r g e t t r a c k i n g . L a s t b u t n o w l e a s t , t h e q u a l i t y o f t h e s e n s o r s t h e m s e l v e s a f f e c t t a r g e t t r a c k i n g . A l l a b o v e e x a m p l e s c o m p o u n d t o p r o d u c e a v e r y n o i s y s e n s o r o u t p u t . F o l l o w i n g p u b l i c a t i o n s a r e m o r e r e l e v a n t t o t h i s p r o j e c t a m o n g e x i s t i n g w o r k o n r e l a t e d s u b j e c t s , S t a n c i u , P a u l Y O h ( 2 0 0 2 ) s t u d i e d v i s u a l l y s e r v o e d t r a c k i n g t o a u g m e n t c a m e r a t e l e - o p e r a t o r s . W h e r e t h e y d e s i g n e d c o n t r o l s y s t e m s t o k e e p t h e t a r g e t p o s i t i o n e d a t t h e c e n t e r o f t h e i m a g e a u t o m a t i c a l l y a u g m e n t i n g t h e c a m e r a o p e r a t o r s i n p u t s . B h a r a t N . S h a h , P a u l Y O h ( 2 0 0 2 ) s t u d i e d b i o m e t r i c v i s u a l s e r v o i n g u s i n g a p a r t i t i o n e d c o n t r o l s c h e m e . T h i s s t u d y p r e s e n t e d a p a r t i t i o n e d c o n t r o l l e r f o r t h e v i s u a l l y s e r v o e d t r a c k i n g o f o b j e c t s u s i n g b o t h i m a g e a n d k i n e m a t i c s d a t a . R e s u l t i n g d e s i g n s h o w e d s i m i l a r p e r f o r m a n c e t o v i s i o n s y s t e m s i n b i o l o g y . S i s i l K u m a r a w a d u , K i e g o W a t a n a b e ( 2 0 0 7 ) s t u d i e d h i g h p e r f o r m a n c e O b j e c t t r a c k i n g a n d f i x a t i o n u s i n g a r o b o t i c h e a d w i t h s t e r e o v i s i o n . . f 1 . 2 M o t i v a t i o n M o t i v a t i o n f o r t h e p r o j e c t w a s b o r n o u t o f t h e u n i q u e c o n t r o l s y s t e m s c h a l l e n g e p r e s e n t e d b y t h e p r o c e s s o f t a r g e t t r a c k i n g . T h i s p r o j e c t r u n s t h r o u g h t h e e n t i r e g a m u t o f t o p i c s c o v e r e d d u r i n g t h e P o s t G r a d u a t e D i p l o m a s t a g e . T h e w r i t e r a c k n o w l e d g e s t h a t p r o j e c t w a s n o t u n d e r t a k e n t o s o l v e a p a r t i c u l a r i m m e d i a t e p r o b l e m b u t t o g a i n e x p e r i e n c e a n d d e v e l o p P r o c e d u r e s u s i n g e x i s t i n g t e c h n o l o g i e s t o s o l v e t h e m u c h c o m p l e x p r o b l e m o f a m i l i t a r y g r a d e l o w l e v e l a n t i - a i r c r a f t a r t i l l e r y s y s t e m , c a p a b l e o f e n g a g i n g f i x e d w i n g a i r c r a f t , h e l i c o p t e r s a n d u n m a n n e d a e r i a l v e h i c l e s ( U A V ) . H e n c e t h e s c a l e d d o w n l a b o r a t o r y m o d e l o f t h e l i f e s i z e s y s t e m . F u r t h e r m o r e t h e r e a r e s e v e r a l i m p e d i m e n t s i n d e v e l o p i n g a f u l l s c a l e v e r s i o n . S u c h a s f u n d i n g a n d r e g u l a t o r y r e s t r i c t i o n s b e i n g a m i l i t a r y a p p l i c a t i o n . _ , _ S r i l a n k a , b e i n g a n i s l a n d n a t i o n w i l l h a v e t o g u a r d i t s e l f a g a i n s t a i r b o r n e e n e m i e s i n t h e f u t u r e . W e h a v e s e e n i n t h e p a s t t h a t o u r d e f e n s e s a r e m o s t v u l n e r a b l e t o a i r b o r n e a t t a c k s o f a n y n a t u r e . O u r e n e m i e s n e e d n o t a c q u i r e h i g h l y s o p h i s t i c a t e d a i r c r a f t t o t h r e a t e n t h e v e r y h e a r t o f S r i L a n k a . T h e r e f o r e i t i s p r u d e n t t o b u i l d u p o u r a i r d e f e n s e s f o r s u c h a t t a c k s i n t h e f u t u r e . I n t h i s E n d e a v o u r c o u n t r i e s l i k e o u r s f a c e t h e f o r m i d a b l e o b s t a c l e o f f o r e i g n e x c h a n g e o u t f l o w i n t h e f o r m o f d e f e n s e e x p e n d i t u r e . A i r d e f e n s e s y s t e m s c o s t l a r g e a m o u n t s o f m o n e y w h i c h w e c a n i l l a f f o r d . T h e r e f o r e , i t i s u n w i s e n o t t o c o n s i d e r a s y s t e m o f o u r o w n , e s p e c i a l l y g i v e n t h e f a c t t h a t t h e t e c h n o l o g y r e q u i r e d i s m o r e o r l e s s a v a i l a b l e a n d a c c e s s i b l e ( a l a s i n f r a g m e n t e d f o r m ) . F u r t h e r m o r e , i f s u c h a s y s t e m c o u l d b e d e v e l o p e d , i t c o u l d b e c o m m e r c i a l i z e d t o b r i n g i n v a l u a b l e f o r e i g n e x c h a n g e i n t h e f u t u r e . T h o u g h s u c h s y s t e m s a r e a v a i l a b l e i n t h e m i l i t a r y h a r d w a r e m a r k e t , s u c h a s t h e R u s s i a n Z S U - 2 3 / 4 ( S h i l k a ) 2 3 m m q u a d a u t o m a t i c S e l f P r o p e l l e d A n t i - A i r c r a f t ( S P A A ) g u n , r a r e l y d o e s t h e t e c h n o l o g y m a d e a v a i l a b l e f r e e l y i n t h e f o r m o f b l u e p r i n t s a s s u c h e a c h c o u n t r y n e e d s t o d e v e l o p i t s o w n c u l t u r e o f m i l i t a r y h a r d w a r e t e c h n o l o g y . 2 i . 1 . 3 G o a l s a n d A c h i e v e m e n t s T h e m a i n g o a l o f t h i s p r o j e c t i s t o d e v e l o p t h e t e c h n o l o g i e s r e q u i r e d f o r t h e f u l l s c a l e t a r g e t t r a c k i n g A A A s y s t e m o f a c o m p l e x a i r d e f e n s e s y s t e m a n d t o i m p l e m e n t a s c a l e d d o w n p r o t o t y p e . T o t h i s e n d t h e d e s i g n e r s e e k s t o d e v e l o p c l e a r l y i d e n t i f i a b l e p r o c e d u r e s f o r t h e p u r p o s e o f ; • S y s t e m m o d e l i n g t e c h n i q u e s • C o n t r o l s y s t e m s ; • M o t o r D r i v e r s a n d E l e c t r o n i c s • S y s t e m i m p l e m e n t a t i o n u s i n g e m b e d d e d s y s t e m s f o r r e a l t i m e c o n t r o l • T a r g e t c h a r a c t e r i s t i c s a n a l y s i s e t c . I n o r d e r t o a c h i e v e a f o r e m e n t i o n e d g o a l s , w e n e e d e d a p a n - t i l t b a s e w h i c h i s c a p a b l e o f p e r f o r m i n g a s a l a b o r a t o r y m o d e l f o r t h e l i f e s i z e s y s t e m a n d d i s p l a y s t h e d y n a m i c c h a r a c t e r i s t i c s o f a l i f e s i z e s y s t e m . T h e r e f o r e t h e l a b o r a t o r y m o d e l n e e d t o b e f a i r l y l a r g e e n o u g h s u c h t h a t i t i n c o r p o r a t e s a l l t h e m e c h a n i c a l c h a r a c t e r i s t i c s a s s o c i a t e d w i t h a r e a l s y s t e m s a n d i n s i g n i f i c a n t q u a n t i t i e s , i . e : c o u p l i n g e f f e c t s , c a r i o l e s f o r c e , f r i c t i o n , g r a v i t y e t c . T h e r e f o r e t h e d e v e l o p m e n t o f s u c h a s y s t e m i s o n e o f t h e p r i m a r y g o a l s o f t h e p r o j e c t . F a l l o w i n g m i l e s t o n e s i n r e l a t i o n t o t h e g o a l s s t a t e d a b o v e h a v e b e e n a c h i e v e d . • A f u l l y f u n c t i o n a l p a n t i l t m e c h a n i s m • A s e r v o c o n t r o l l e r h a v i n g t w o a n a l o g i n p u t s t w o d i g i t a l i n p u t s a n d t w o a n a l o g o u t p u t s ( P W M ) o S e r v o c o n t r o l l e r i s c a p a b l e o f d r i v i n g 8 a m p e r e s t o t w o m o t o r s a t 2 4 V D C o S e r v o c o n t r o l l e r i s c a p a b l e o f I n C i r c u i t S e r i a l P r o g r a m m i n g • A m a c h i n e v i s i o n s y s t e m f o r t a r g e t i d e n t i f i c a t i o n . • I n t e r f a c e b e t w e e n T a r g e t t r a c k i n g s y s t e m a n d P a n t i l t p l a t f o r m . . - • S i m u l i n k m o d e l o f t h e s y s t e m • S o l i d w o r k s m o d e l f o r c a l c u l a t i n g i n e r t i a v a l u e s . / • ) - '~::.f.:¥ ~ J > ' j : \ , , ; _ • " . . . * ; _ f f , . · : , r ·iS~Y:ff' • B o d e a n a l y s i s o f t h e P a n - T i l t p l a t f o r m ""'-'-~·~ . . . ··~~?~- • T a r g e t c h a r a c t e r i s t i c s u s i n g F a s t F o u r i e r T r a n s f o r m a t i o n t e c h n i q u e s • F i e l d t e s t o f v i s i o n b a s e d t a r g e t t r a c k i n g s y s t e m . • T h r o u g h a b o v e a c h i e v e m e n t s , t h e a u t h o r w a s a b l e t o c o n f i r m t h a t t h e d e s i g n p r o c e d u r e i s v a l i d a n d c a n y i e l d u s a b l e r e s u l t s . 3 C h a p t e r 2 T a r g e t I d e n t i f i c a t i o n a n d T r a c k i n g T e c h n i q u e s A n y d e s i g n e r i s s p o i l t f o r c h o i c e w h e n i t c o m e s t o t e c h n o l o g i e s a v a i l a b l e f o r i d e n t i f y i n g a t a r g e t . P r o m i n e n t a m o n g t h e m a r e ; R a d a r , i n f r a r e d i m a g i n g , t h e r m a l i m a g i n g , v i s i o n a n d u l t r a s o u n d e t c . W h e n i t c o m e s t o i d e n t i f y i n g a t a r g e t a t l o n g d i s t a n c e s m o s t o f t h e s e c h o i c e s f a i l t o a c h i e v e r e l i a b l e r e s u l t s , f o r i n s t a n c e , i n f r a r e d i m a g i n g , t h e r m a l i m a g i n g a n d v i s i o n a l l , . . r e q u i r e t h e t a r g e t t o b e w i t h i n l i n e o f s i g h t f r o m t h e t r a c k i n g c a m e r a o F u r t h e r m o r e t h e s e t r a c k i n g c a m e r a s s t r u g g l e t o p i c k u p a n y t h i n g b e y o n d 5 0 0 m e t e r s . T h e r e f o r e i t o n l y l e a v e s o n e c a n d i d a t e t o t r a c k l o n g d i s t a n c e t a r g e t s i . e ; R a d a r . C o m m e r c i a l a v i a t i o n i s u s e s a t e c h n o l o g y c a l l e d " s e c o n d a r y s u r v e i l l a n c e r a d a r " o r ( S S R ) . T h i s s y s t e m t r a c k s t h e t a r g e t b y l o c k i n g o n t o a b e a c o n s e n t o u t b y t h e a i r c r a f t b e i n g t r a c k e d . B u t i t i s o b v i o u s t h a t w e c a n n o t p u t b e a c o n s o n a n e n e m y a i r c r a f t . T h e r e f o r e m i l i t a r y a v i a t i o n i s h a s t o w o r k w i t h p r i m a r y r a d a r . T h e p r o p o s e d s y s t e m u s e s a r e m o t e p r i m a r y r a d a r u n i t f o r t a r g e t t r a c k i n g a n d i d e n t i f i c a t i o n ; h o w e v e r a t t h i s s t a g e o f d e v e l o p m e n t t h e a u t h o r i s u n a b l e t o a c q u i r e t a r g e t i n f o r m a t i o n f r o m a r e a l r a d a r s y s t e m . T h e r e f o r a c o m p u t e r v i s i o n s y s t e m c a p a b l e o f d e l i v e r i n g t a r g e t i n f o r m a t i o n s i m i l a r t o t h a t o f p r i m a r y r a d a r i s u s e d . T h i s c o m p u t e r v i s i o n s y s t e m f e e d s p a n a n d t i l t a n g l e d a t a t o t h e t u r r e t s y s t e m . S o m e k i n d o f i n t e l l i g e n t s y s t e m o r a h u m a n i n t e r f a c e i s r e q u i r e d b e t w e e n t h e t r a c k i n g s y s t e m a n d t h e g u n t u r r e t t o i d e n t i f y f r i e n d o r f o e . H o w e v e r , d e s i g n o f s u c h a s y s t e m i s n o t c o v e r e d w i t h i n t h i s p r o j e c t I n s u m m a r y t h e c o m p l e t e s y s t e m c o n s i s t s o f l . c o m p u t e r v i s i o n s y s t e m f o r l a b o r a t o r y m o d e l , 2 . I n t e l l i g e n t f r i e n d o r f o e i d e n t i f i c a t i o n s y s t e m , 3 . G u n t u r r e t . A t t h i s p o i n t i t i s p r u d e n t t o p o i n t o u t t h a t t h e s y s t e m i s a c l o s e d l o o p s y s t e m , W i t h a n y c l o s e d l o o p s y s t e m , t h e b a n d w i d t h o f t h e s y s t e m i s d e t e r m i n e d b y t h e s l o w e s t c o m p o n e n t i n t h e l o o p . I n t h i s c a s e i t i s t h e r a d a r d e t e c t i o n s y s t e m . 4 ~ 2 . 1 S y s t e m o v e r v i e w T h e s y s t e m i s c o n s i s t e d o f a t r a c k i n g s y s t e m , i n t e l l i g e n t f r i e n d o f f o e i d e n t i f i c a t i o n s y s t e m a n d t h e p a n - t i l t a n t i a i r c r a f t g u n t u r r e t . ( 0 - J \ . . <,~; ' . . . , 1 ~· I r a c k t t l c ' ~~,.>- \ ; l n k l l i : , : . . : t l l I r i , · t t d t H j ( , _ . i c l . . : n l i lic·~ll i P I I ' ) ' ! c l l l 1 , · , , 1 k t d . ( ! ' d l l I i I t ~IIi.:';,, I ( t > c > : \ l i n : : : , · I r a t : : - t ( > r t u , l l i ' ' ' ' d l l l l j 1 l l \ \ l ' l d l l i j ' l i l i c r - ' F i g 2 . 1 A u t o m a t e d t a r g e t t r a c k i n g a i r d e f e n s e s y s t e m 2 . 2 C o n t r o l T e c h n o l o g i e s : • . . D l ' d l i - I i ! t d r i ' . . _ . l l h . \ l t \ t : - . T h e r e a r e s e v e r a l w a y s b y w h i c h t h e p a n - t i l t b a s e c a n r e s p o n d t o t h e e r r o r b e t w e e n t h e d e s i r e d a n g l e a n d t h e a c t u a l a n g l e . I t c a n s w i t c h O N o r O F F t h e m o t o r i n e i t h e r d i r e c t i o n d e p e n d i n g o n w h e t h e r t h e e r r o r i s p o s i t i v e o r n e g a t i v e . T h i s i s c a l l e d s i m p l e O N / O F F c o n t r o l o t h e r w i s e i t c a n p r o d u c e a c o n t r o l s i g n a l w h i c h i s p r o p o r t i o n a l t o t h e e r r o r t h i s i s p r o p o r t i o n a l ( P ) c o n t r o l . 5 I f i t p r o d u c e s a c o n t r o l s i g n a l p r o p o r t i o n a l t o t h e r a t e o f c h a n g e o f t h e e r r o r i t i s c a l l e d D e r i v a t i v e ( D ) c o n t r o l . l t c a n a l s o p r o d u c e a c o n t r o l s i g n a l w h i c h i s p r o p o r t i o n a l t o t h e i n t e g r a l o f t h e e r r o r w i t h t i m e a n d t h i s i s i n t e g r a l ( I ) c o n t r o L T h e c o n t r o l l e r c a n a l s o u s e c o m b i n a t i o n s o f P , I o r D c o n t r o l . H o w e v e r o t h e r o p t i o n s a v a i l a b l e t o t h e d e s i g n e r a r e p a r t i t i o n e d P D c o n t r o l , a d a p t i v e c o n t r o l , c o m p u t e d t o r q u e c o n t r o l a n d i n v e r s i o n b a s e d c o n t r o l [ 6 J , [ 7 ] , [ 8 ] , a l l o f w h i c h h a s d e m o n s t r a t e d b e t t e r c o n t r o l p e r f o r m a n c e o v e r P I D c o n t r o L H o w e v e r f o l l o w i n g s e c t i o n e x a m i n e s a d v a n t a g e s a n d d i s a d v a n t a g e s o f u s i n g e a c h t y p e o f c o n t r o l t e c h n o l o g y t o a r r i v e a t t h e m o s t a p p r o p r i a t e t y p e o f c o n t r o l t e c h n o l o g y f o r t h e s y s t e m . - ' l ' a n .\n~iL' < -~ l f l l r t 1 ] I L " r . l o i n ! ( · P n l l · u l < ~ l · c · c ' c ! l ' l a c k I . . . . . \ ~ [ C 1 1 1 - < = = = = t = : J F i g . 2 . 2 S y s t e m M o d e l J ~It . \ n g l c ( · , 1 1 1 t r t 1 ] l c r I t i s c l e a r t h a t o u r s y s t e m i s a m u l t i i n p u t m u l t i o u t p u t s y s t e m ( M I M O ) . t o s i m p l i f y t h e p r o b l e m e a c h j o i n t ( P a n a n d t i l t ) i s c o n s i d e r e d s e p a r a t e l y a n d a s s u m e d t o b e i n d e p e n d e n t . T h e r e f o r e w e c o v e r t t h e a b o v e M I M O s y s t e m i n t o t w o S i n g l e i n p u t s i n g l e o u t p u t s y s t e m s ( S I S O ) . C o n t r o l s c h e m e s c o n s i d e r e d f o r t h i s s y s t e m i n c l u d e , P I D c o n t r o l o r p r o p o r t i o n a l I n t e g r a l a n d d e r i v a t i v e c o n t r o L 6 ~ I n P I D c o n t r o l , a l l n o n - l i n e a r e f f e c t s a r e c o n s i d e r e d a s n o i s e _ P I D c o n t r o l s c h e m e h a s t h e u n i q u e c a p a b i l i t y o f o v e r c o m i n g a l l s u c h n o i s e i n p u t s , g i v e n a d e q u a t e b a n d w i d t h i s a v a i l a b l e w i t h t h e s y s t e m . T h e r e f o r e t h e u l t i m a t e j u s t i f i c a t i o n f o r u s e o f P I D c o n t r o l s c h e m e i s d e p e n d e n t o n t h e f i n a l o u t c o m e o f t h e s y s t e m , i . e , i f t h e s y s t e m i s c a p a b l e o f a c h i e v i n g s p e c i f i e d p e r f o r m a n c e b e n c h m a r k s , u s e o f P T D c o n t r o l l e r i s j u s t i f i e d . T h e u s e o f l i n e a r c o n t r o l s c h e m e s i s o n l y v a l i d f o r t h o s e s y s t e m s w h o s e b e h a v i o r c a n b e m a t h e m a t i c a l l y m o d e l e d b y l i n e a r d i f f e r e n t i a l e q u a t i o n s [ 6 ] . L i n e a r c o n t r o l t e c h n i q u e s f o r m a n i p u l a t o r c o n t r o l a r e e s s e n t i a l l y a n a p p r o x i m a t e m e t h o d a n d f o r t h i s a m a n i p u l a t o r j o i n t s h o u l d b e m o d e l e d a s a l i n e a r s e c o n d - o r d e r s y s t e m . - ' 2 . 3 L i n e a r S e c o n d O r d e r S I S O M o d e l o f a M a n i p u l a t o r J o i n t . P a n a n d t i l t j o i n t s o f t h e m e c h a n i s m i s c o n s i d e r e d a s a m a n i p u l a t o r j o i n t a n d l i n e a r s e c o n d o r d e r m o d e l i s g i v e n b e l o w [ 6 ] A c t u a t o r 1 b _ l ' d u c t i o n ' · · · a r s l J C L i n k L i n I . ; . F i g 2 . 3 L i n e a r s e c o n d o r d e r S I S O m o d e l o f a m a n i p u l a t o r j o i n t n u m b e r o f t e e t h o n t h e g e a r o f l i n k s h a f t l J 1 n - - - · t - - n u m b e r o f t e e t h o n t h e g e a r o f a c t u a t o r s h a f t l J 7 T h e g e a r r a t i o n 1 J i s g r e a t e r t h a n 1 f o r a r e d u c t i o n g e a r . T h e g e a r T = ' l J T a r ( 2 . 1 ) ( 2 . 2 ) 7 1 e = - e a ( 2 . 3 ) T J e = _ ! _ e ( / ( 2 A ) T J T h e l i n k t o r q u e r w h e n r e f l e c t e d t o t h e a c t u a t o r s i d e , i s s c a l e d d o w n b y a f a c t o r o f T J , t h e g e a r r a t i o . I a i s t h e i n e r t i a l o f r o t o r o f t h e a c t u a t o r a n d B a i s t h e v i s c o u s f r i c t i o n c o e f f i c i e n t a t a c t u a t o r b e a r i n g s , f o r t h e r o t a t i o n a l m a s s - d a m p e r s y s t e m , t h e t o r q u e r e q u i r e d a t t h e a c t u a t o r s h a f t i s g i v e n b y r r a = I " 8 " + B " 8 " + - T J 1 r " = I a 8 a + B a 8 a + - ( 1 8 + B 8 ) 1 J . : ( 2 . 5 ) ( 2 . 6 ) W h e r e I a i s t h e i n e r t i a o f t h e l o a d a n d B i s t h e v i s c o u s f r i c t i o n c o e f f i c i e n t f o r t h e l o a d . I n t e r m s o f a c t u a t o r v a r i a b l e s , r " + " + ; , ) o , + ( R , + ; ) e , ( 2 . 7 ) I n t e r m s o f l i n k v a r i a b l e s , r = ( I + 7 7 2 I " ) e + ( B + 7 7 2 B " ) e ( 2 . 8 ) T h e l i n k t o r q u e r i s o b t a i n e d f r o m t h e d y n a m i c m o d e l o f t h e j o i n t T h e d y n a m i c e q u a t i o n o f a j o i n t I o f a m a n i p u l a t o r i s g i v e n b y ; r , = L M u q 1 + L L h u k q 1 q , + G , ( 2 . 9 ) k E q ( 2 . 9 ) e q u a t i o n i s r e w r i t t e n b e l o w w i t h t h e s u b s c r i p t i f o r j o i n t i d r o p p e d f o r c o n v e m e n c e . r = " L M 1 8 1 + L L h ; k 8 1 e k + G j k ( 2 J O ) W h e r e G i s t h e G r a v i t y t e r m a n d o n l y c o m e s i n t o e f f e c t w h e n m o t i o n i s a g a i n s t g r a v i t y . N o t e t h a t t h e e q u a t i o n d o e s n o t i n c l u d e t h e e f f e c t s o f f r i c t i o n , b a c k l a s h a n d e l a s t i c d e f o m 1 a t i o n s . 8 T h e v i s c o u s f r i c t i o n i s d i r e c t l y p r o p o r t i o n a l t o t h e v e l o c i t y a n d c a n b e i n c l u d e d i n t h e m o d e l . r = ' L M I e I + I I h l j k e I e k + B e + G ( 2 _ 1 1 ) I k W h e r e B i s t h e v i s c o u s f r i c t i o n c o e f f i c i e n t a t t h e l i n k b e a r i n g s , T h e t e r m s i n a b o v e e q u a t i o n a r e s p l i t i n t o l i n e a r d e c o u p l e d a n d n o n l i n e a r i n t e r a c t i n g t e r m s . T h e t w o l i n e a r d e c o u p l e d t e r m s a r e M e f r o m t h e f i r s t s u m m a t i o n t e r m a n d t h e t h i r d t e r m B e . T h e r e m a i n i n g t e r m s a r e n o n l i n e a r i n t e r a c t i n g t e r m s a n d a r e g r o u p e d t o g e t h e r a n d d e n o t e d a s ' d ' t h a t i s 1 7 d = I M 1 e l + I ' L h 1 k e l e k + G ( 2 . 1 2 ) 1~1 1 k I " ' E q u a t i o n 2 . 1 1 i s w r i t t e n a s ; r = M 8 + B 8 + d ( 2 . 1 3 ) S u b s t i t u t i n g r f r o m E q . ( 2 . 1 0 ) i n t o E q . ( 2 . 5 ) a n d r e a r r a n g i n g , t h e a c t u a t o r t o r q u e i s o b t a i n e d a s ; r = ( I + ! i - J e + ( B + ! ! _ J e + 9 _ a u 2 a o 2 a r y r y r y ( 2 , 1 4 ) S i n c e t h e p a n t i l t m e c h a n i s m i s h i g h l y g e a r e d , T l = 8 0 f o r p a n m e c h a n i s m a n d T l = 6 6 f o r t i l t m e c h a n i s m i t i s r e a s o n a b l e t o i g n o r e t h e c o n t r i b u t i o n o f t h e n o n l i n e a r i n t e r a c t i n g t e r m d a n d s t i l l g u a r a n t e e a g o o d t r a j e c t o r y - t r a c k i n g p e r f o r m a n c e . N e g l e c t i n g n o n l i n e a r t e r m s , t h e a c t u a t o r t o r q u e r e d u c e s t o ; T ( / = J e f f 8 ( / + B e f ! e ( / ( 2 . 1 5 ) W h e r e I e f f i s t h e e f f e c t i v e i n e r t i a a n d B e g i s t h e e f f e c t i v e d a m p i n g w i t h ; 1 . 1 " = I + ~ L _ ; a 2 r y ( 2 . 1 6 ) B e f f = B " + ~2 ( 2 , 1 7 ) 9 L o a d t o r q u e c a n b e w r i t t e n a s ; T = J e f f 8 + B c f f 8 ( 2 . 1 8 ) S i n c e t h e c o n f i g u r a t i o n d e p e n d e n t t e r m ' M ' i s r e d u c e d b y a f a c t o r 7 7 2 , w i t h r J > > 1 , f e J J i s a l m o s t c o n s t a n t f o r h i g h l y g e a r e d m a n i p u l a t o r s . T h e r e f o r e t h e i n e r t i a t e r m s e e n b y t h e s y s t e m i s e f f e c t i v e l y t h e i n e r t i a t e r m o f t h e a r m a t u r e I a . A n d w e w i l l h a v e t o o b t a i n B e J J u s i n g e x p e r i m e n t a l m e t h o d s . . : 2 . 4 M o t o r m o d e l R !~' y • e = K 8 T f t 8 \ F i g 2 . 5 m o t o r m o d e l d i L - ! ' _ + R i = v - K 8 ( i d t { I ( I I ! ( / ( 2 . 1 9 ) e = K 8 e a ( 2 . 2 0 ) A n i n e r t i a l l o a d r i s c o n n e c t e d t o t h e m o t o r a r m a t u r e v i a a g e a r b o x w i t h r a t i o r y . T h e c o n f i g u r a t i o n i s i l l u s t r a t e d i n f i g 2 . 5 T = J e f f 8 + B c j f 8 ( F r o m 2 . 1 8 ) T a = K ) a ( 2 . 2 1 ) ( 2 . 2 2 ) A b o v e e q u a t i o n s c a n b e u s e d t o c o n s t r u c t a s i g n a l f l o w g r a p h o f t h e e l e c t r i c m o t o r / l o a d w h i c h c a n b e r e a l i z e d i n S I M U L I N K . 1 0 I I . C h a p t e r 3 I m p l e m e n t a t i o n o f V i s i o n B a s e d T a r g e t T r a c k i n g S y s t e m I m p l e m e n t a t i o n c o n s i s t s o f d e s i g n a n d c o n s t r u c t i o n o f t h e T u r r e t b a s e a n d i n t e g r a t i n g i t w i t h t h e m a c h i n e v i s i o n s y s t e m - ' 3 . 1 D e s i g n a n d C o n s t r u c t i o n o f T u r r e t B a s e - - . - , _ ·~;-_:t~ w i d e a n g l e t r a c k i n g C a m e r a o r Radar~ ; ' ( n - - - - - T i l t M o t o r ¥'~~ ,~1,":1~ - _ I l~ 1 0 0 5 0 0 A - 1 0 T h u n d e r b o l t m o v e m e n t p r o f i l e " "r~ ,-~ T " - T · " " " " P ' ' ' " ' , I I I I I ! ' I I . 1 ' ~ ·~ . . . . . . / / T I + · , I I I I I I r : i - J - . I ! . ' ' I _ i L ' ' r / I I l i I I I I I ' I o N ~ ~ r o o N ~ ~ r o " ' \ " " " " ~ ~ ~ T " " " ' T i m e . ;·-vel~"~i~~~~"~E~Iy1v~~~~] F i g 4 . J v e l o c i t y p r o f i l e o f a n A 1 0 t h u n d e r b o l t a t m a x i m u m a c c e l e r a t i o n T h e t h i r d o r d e r v e l o c i t y p r o f i l e p o l y n o m i a l o f t h e a b o v e m o v e m e n t t a k e s t h e f o r m ; V = a t 3 + h t 2 + c t + d ( 4 . 1 ) T h e r e f o r e c o e f f i c i e n t s c a l c u l a t e d u s i n g r e g r e s s i o n t e c h n i q u e s a r e ; a = 0 . 1 6 h = - 5 . 6 3 c = 5 9 . 4 9 d = - 8 . 3 6 2 2 E Q ) u c r o u ; ' 6 L B y i n t e g r a t i n g t h e v e l o c i t y p r o f i l e o n c e o v e r t i m e , w e e n d u p w i t h t h e p o s i t i o n m o v e m e n t p r o f i l e . I e : 1 6 0 0 1 - - - T - - r 1 4 o o 1 1 2 0 0 r 1 0 0 0 1 1 8 0 0 1 c I 6 0 0 ! 4 0 0 r f 2 0 0 - o f - 2 0 0 ~ 0 y r + . t 3 a - + b - 4 3 t 2 + c - + d t + c o n s t a n t 2 Y = 0 . 0 4 t 4 ~ \ . 8 8 t 3 + 2 9 . 7 5 t 2 ~ 8 . 3 6 t + c o n s t a n t v a r i a t i o n o f d i s t a n c e w i t h t i m e ( A 1 0 - t h u n d e r b o l t ) I I I 2 I - 1 - - 1 - I . . _ ! _ , - · · -~- I I . 1 3 4 - r - - 1 I ~ I I I I < I - - I I 5 t i m e ( s ) - - - , - - - - - - , - - - - - - - ; - - - - 1 I I l _ 6 7 8 - · 9 I 1 - I I - I I I 1 ! -~ 1 0 F i g 4 . 2 m o v e m e n t p r o f i l e o f t h e A - 1 0 t h u n d e r b o l t a t m a x i m u m a c c e l e r a t i o n A b o v e e q u a t i o n i s i n t h e f o r m : . Y = A t 4 t B t 3 + C t 2 + D t + E B u t w e k n o w E = 0 ( i n i t i a l v e l o c i t y ) H e n c e ; A = 0 . 0 4 B = - r 8 8 C = 2 9 . 7 5 D = - 8 . 3 6 E = 0 ( 4 . 2 ) ( 4 . 3 ) 2 3 A b o v e m o v e m e n t p r o f i l e ( F i g 2 . 6 ) i s o b s e r v e d f r o m a d i s t a n c e o f 3 0 0 0 m e t e r s f r o m t h e t a r g e t w h i c h i s w e l l w i t h i n t h e t a r g e t i n g c a p a b i l i t i e s o f P G Z 9 5 ! \ h n ~..·tnL'lll P a t h _ ) ( ) ( ) ( ) I l l - ' • F i g 4 . 3 m o v e m e n t o b s e r v e d f r o m a d i s t a n c e o f 3 0 0 0 m P a n a n g l e o f t h e o b s e r v i n g b a s e w i l l v a r y a s f o l l o w v a r i a t i o n o f t h e t a w i t h t i m e 9 6 I I I 9 4 9 2 I f ) Q ) 9 0 (~:! ( J ) Q ) ' " 0 c 8 8 Q ) O l c r o 8 6 8 4 8 2 ; J I I T - - 1 l l 1 - T I T I I I J - ' I j _ I j - 1 - - I + I l 1 - T - I T - 1 1 I I I I I . J 1 - J _ 1 - 1 I I I I I ' I I I I I I T - - - ~--~-~-~~------;--I---T--1 ---~-~------- 1 I I I - T - - I r I r I I I I I I 1 _ I _ L _ I j - I I t - 1 - t - 1 I l I I _ I _ _ I I _ I I - r I I ' I I i L . J I I _ J I J I I - - ( - 1 - 1 I - : - I -~-- - 1 - ' - 1 I I L _ j - L 1 - - - - 1 I I I I - I I I I I I t I - 1 - f . - I f . - ; - 1 - 1 - - 1 - f . - - i ~ - 1 - I _ l _ l I I I _ ~ _ I _ I I I I T -~-I - 1 I I I I I 1 I _ L _ I l _ I L I I I 1 I I I T -~ T -~ I I I I I ' t - 1 t - I - f - I I j l I I I _ t _ l _ I _ I _ ~ _ I I I I I I I I I I I I I L I - ' I I ~- I - - I 1 . I t I l I - 1 1 - - 1 - 1 - + I - I I - I _ ~ ~-- l 1 I 1 - 1 - T - I 7 " - - 1 - 7 " I T - I r - I : - 1 1 l - I l I l - l I T - I T 8 0 u I I t I _ J . _ _ _ _ j _ ~- ~-~ ~--~-~-_l_~--~-~ _ j I r I I I I I I I I I I . L _ . J - _ j _ _ _ l _ _ l _ _ l _ _ _ _ _ l _ _ _ l _ _ _ j _ _ _ L _ _ j _ _ _ l _ _ _ L _ _ _ t _ _ _ t _ _ _ l 0 2 3 4 5 6 7 t i m e i n s e c o n d s F i g 4 , 4 V a r i a t i o n o f p a n a n g l e w h e n t h e m o v e m e n t i s o b s e r v e d f r o m a d i s t a n c e o f 3 0 0 0 m 2 4 F F T a n a l y s i s ( s e e A p p e n d i x C f o r . m f i l e f o r f f t a n a l y s i s ) i s d o n e o n t h e a b o v e m o v e m e n t ( f i g . 4 . 4 ) p r o f i l e t o o b t a i n t h e m a x i m u m f r e q u e n c y c o m p o n e n t s o f t h e p r o f i l e ; u s i n g a n . m f i l e ( s e e a p p e n d i x C ) f o l l o w i n g p o w e r s p e c t r u m p l o t i s o b t a i n e d f o r t h e a b o v e m o v e m e n t . x ~§Wer S p e c t r u m o f a J e t a i r c r a f t ( a n g u l a r v a r i a t i o n ) o b s e r v e d f r o m 3 0 0 0 m 3.5~~~:~-:--,----=-=i~~~~~-~-~----,.-T- ~--=--- . . . , ~-;----~ ~T=-, · · - I - I 0 . 5 - I I _ I _ _ I _ I _ I I I -~- -~- - 1 - 1 _ I _ I -~- I · I - I 1 - _ 1 _ . I . I I -~- -~- _ I _ I - - 1 I -~- I - 1 - I I I -~- -~ - -~ - I - 1 - - - j - - j - J - J - - -~- - 1 · j - I - I - - 1 - J - -~- , _ J _ l _ I 1 - I - - I - r - - - - - 1 - I - - - - 1 - 0 ' _ _ _ _ _ _ _ l _ _ _ _ _ _ - - - ' - - _ _ l _ _ _ j _ _ _ ~ _ _ _ _ I _ _ l _ _ _ _ j 0 0 . 1 0 . 2 0 . 3 0 . 4 0 . 5 0 . 6 F r e q u e n c y ( H z ) F i g 4 . 5 P o w e r s p e c t r u m o f t h e t a r g e t m o v e m e n t I + - ' ' - j I T - ' l - c I _ I -~ t - - - t - c - I I I - r I - - I I I I - - T I - - - - l t j - - t - 1 l _ L _ _ L 1 - l - - l t t - l l I j 0 . 7 0 . 8 - I - - f - - t - L - T - c - t L _ L I - I 0 . 9 I - r F r o m f i g . 4 . 5 i t i s c l e a r t h a t f r e q u e n c y c o m p o n e n t s a b o v e 1 H z a r e i n s i g n i f i c a n t . T h e r e f o r e , p a n t i l t m e c h a n i s m s h o u l d p o s e s a m i n i m u m b a n d w i d t h o f 1 H z ( 2 n r a d / s ) o r m o r e . T h i s b a n d w i d t h i s m u l t i p l i e d b y t h e g e a r s r a t i o t o o b t a i n t h e m o t o r a r m a t u r e b a n d w i d t h . T h e r e b y g i v i n g a v e l o c i t y b a n d w i d t h o f 1 4 1 . 6 r r r a d / s S a m e p r o c e d u r e i s c a r r i e d o u t t o o b t a i n t h e t i l t m e c h a n i s m v e l o c i t y b a n d w i d t h a s 1 5 5 r r ( r a d / s ) ; C a l c u l a t i n g t h e m a x i m u m a n g u l a r a c c e l e r a t i o n o f t h e m o v e m e n t , M a x i m u m a n g u l a r a c c e l e r a t i o n i s i m p o r t a n t t o c a l c u l a t e t h e m a x i m u m t o r q u e e x e r t e d o n t h e m o t o r a r m a t u r e , w h i c h w i l l g i v e a f u r t h e r g u i d e l i n e f o r m o t o r s e l e c t i o n . 2 5 B y d i f f e r e n t i a t i n g t h e a n g u l a r v e l o c i t y m o v e m e n t p r o f i l e o n c e o v e r t i m e , a c c e l e r a t i o n p r o f i l e i s o b t a i n e d ; 3 . 5 3 l C f ) 2 5 r U i 2 f O J ( ! ! O J O J 0 i 1 5 r 0 i l l 1 f > r o 3 O J 0 5 ~ c ( \ 1 o l - 0 5 l 0 v a r i a t i o n o f a n g u l a r ~.elocity w i t h t i m e I _ L 2 - - , - - - _ _ L _ _ _ _ 3 t i m e i n s e c o n d s I I I _ L _ 4 - ' 5 6 F i g 4 . 6 g i v e s t h e a n g u l a r v e l o c i t y p r o f i l e o f t h e a 1 0 , w h e n o b s e r v e d f r o m a d i s t a n c e o f 3 0 0 0 m B y d i f f e r e n t i a t i n g a b o v e m o v e m e n t p r o f i l e o n c e o v e r t i m e , i t i s p o s s i b l e t o a s c e r t a i n t h e a n g u l a r a c c e l e r a t i o n p r o f i l e o f t h e s a i d m o v e m e n t ; I e : v a r i a t i o n o f a n g u l a r a c c e l e r a t i o n w i t h t i m e 1 2 r 1 _ _ _ _ T _ _ _ _ _ T · - - - - T - - r T _ _ T _ _ _ I - - - - - , I 1 1 i l - - I - t I + - + - · - 1 ~ ~·. ~ ~~ ~2 I - - - - I - L _ j -~ N ~ 0 . 9 1 - L - - [ I I I ! 0 8 r - - - t r I c I o l - - r - - r - - I - - - - I - - - - - - -r - - 1 I ~ 0 6 - - - - - - - I - - - - r I - - 1 i o J _ _ - - -~ t o 4 ~ _ I I I I - - - - - - - - - - . I I I I --~1 0 3 f - - : - - - - - - _ _ l _ _ _ l - 0 . 2 _ L _ _ _ , _ _ _ _ 0 . 5 1 1 . 5 2 2 5 3 3 . 5 4 4 5 5 t i m e i n s e c o n d s F i g 4 . 7 a n g u l a r a c c e l e r a t i o n p r o f i l e 2 6 F r o m F i g 4 . 7 i t i s c l e a r t h a t t h e m a x i m u m a c c e l e r a t i o n o c c u r s a t t h e b e g i n n i n g o f t h e m o v e m e n t p r o f i l e a n d i t i s 1 . 0 8 1 d e g r e e s I s 2 ( 0 . 0 1 8 9 r a d / s 2 ) U s i n g t h e s y s t e m i n e r t i a v a l u e o f 0 . 0 0 0 0 1 7 1 6 k g / m 2 , m a x i m u m t o r q u e r e q u i r e d t o a c h i e v e t h i s a c c e l e r a t i o n i s c a l c u l a t e d a s f o l l o w s ; r = J a W h e r e T i s t h e t o r q u e r e q u i r e , J s y s t e m i n e r t i a a n d a i s m a x i m u m a n g u l a r a c c e l e r a t i o n ; B y s u b s t i t u t i n g v a l u e s t o a b o v e e q u a t i o n ; ( 4 . 4 ) T o r q u e 1 i s c a l c u l a t e d a s 0 . 0 0 0 0 0 0 3 2 4 3 2 4 N - m w h i c h i s w e l l w i t h i n t h e t o r q u e c a p a b i l i t i e s o f t h e p a n m o v e m e n t m o t o r ( w h i c h h a s a m a x i m u m t o r q u e o f 0 . 3 3 N - m ) . . : 4 . 1 . 2 A v a i l a b l e B a n d w i d t h o f t h e M e c h a n i c a l S y s t e m T o a s c e r t a i n i t s b a n d w i d t h , s i n u s o i d a l i n p u t s o f i n c r e a s i n g f r e q u e n c y w a s a p p l i e d a n d t h e c o r r e s p o n d i n g r e s p o n s e m e a s u r e d F i g 4 0 8 ( a ) F i g 4 . 8 ( a ) s h o w s t h e s y s t e m r e s p o n s e t o s i n u s o i d a l i n p u t o f 0 . 2 H z , C h a n n e l 3 ( b o t t o m t r a c e ) s h o w s t h e i n p u t s i g n a l , C h a n n e l 1 ( t o p t r a c e ) s h o w s t h e o u t p u t r e s p o n s e . 2 7 . . _ F i g 4 . 8 ( b ) F i g 4 . 8 ( b ) s h o w s t h e s y s t e m r e s p o n s e t o s i n u s o i d a l i n p u t o f 0 . 2 8 H z , C h a n n e l I ( b o t t o m t r a c e ) s h o w s t h e i n p u t s i g n a l , C h a n n e l 2 ( t o p t r a c e ) s h o w s t h e o u t p u t r e s p o n s e . F i g 4 . 8 ( c ) F i g 4 . 8 ( c ) s h o w s t h e s y s t e m r e s p o n s e t o s i n u s o i d a l i n p u t o f 0 . 4 H z , C h a n n e l 1 ( b o t t o m t r a c e ) s h o w s t h e i n p u t s i g n a l , C h a n n e l 2 ( t o p t r a c e ) s h o w s t h e o u t p u t r e s p o n s e . 2 8 L F i g 4 . 8 ( d ) F i g 4 . 8 ( d ) s h o w s t h e s y s t e m r e s p o n s e t o ~inusoidal i n p u t o f 0 . 4 8 H z , C h a n n e l I ( b o t t o m t r a c e ) s h o w s t h e i n p u t s i g n a l , C h a n n e l 2 ( t o p t r a c e ) s h o w s t h e o u t p u t r e s p o n s e . F i g 4 . 8 ( e ) F i g 4 . 8 ( e ) s h o w s t h e s y s t e m r e s p o n s e t o s i n u s o i d a l i n p u t o f 0 . 5 8 H z , C h a n n e l 1 ( b o t t o m t r a c e ) s h o w s t h e i n p u t s i g n a l , C h a n n e l 2 ( t o p t r a c e ) s h o w s t h e o u t p u t r e s p o n s e . 2 9 F i g 4 . 8 ( f ) F i g 4 . 8 ( f ) s h o w s t h e s y s t e m r e s p o n s e t o s i n u s o i d a l i n p u t o f 0 . 7 3 H z , C h a n n e l 1 ( b o t t o m t r a c e ) s h o w s t h e i n p u t s i g n a l , C h a n n e l 2 ( t o p t r a c e ) s h o w s t h e o u t p u t r e s p o n s e . . • " ' . 1 ; . ; ; ; } . - . . t : : \ ~·~' , · : ; ; -'""'i~ ··<~~:!B· ! · i F i g 4 . 8 ( g ) F i g 4 . 8 ( g ) s h o w s t h e s y s t e m r e s p o n s e t o s i n u s o i d a l i n p u t o f 0 . 8 1 H z , C h a n n e l 1 ( b o t t o m t r a c e ) s h o w s t h e i n p u t s i g n a l , C h a n n e l 2 ( t o p t r a c e ) s h o w s t h e o u t p u t r e s p o n s e . 3 0 F i g 4 0 8 ( h ) F i g 4 . 8 ( h ) s h o w s t h e s y s t e m r e s p o n s e t o s i n u s o i d a l i n p u t o f 1 . 1 5 H z , C h a n n e l 1 ( b o t t o m t r a c e ) s h o w s t h e i n p u t s i g n a l , C h a n n e l 2 ( t o p t r a c e ) s h o w s t h e o u t p u t r e s p o n s e . F i g 4 . 8 ( i ) F i g 4 . 8 ( i ) s h o w s t h e s y s t e m r e s p o n s e t o s i n u s o i d a l i n p u t o f 1 . 3 6 H z , C h a n n e l 1 ( b o t t o m t r a c e ) s h o w s t h e i n p u t s i g n a l , C h a n n e l 2 ( t o p t r a c e ) s h o w s t h e o u t p u t r e s p o n s e . 3 1 . . M e c h a n i c a l b a n d w i d t h o f t h e s y s t e m i s o b t a i n e d b y p l o t t i n g t h e g a i n a g a i n s t f r e q u e n c y . I - 2 0 0 1 . 0 0 l I G a i n ( d b ) I I --T~-- 1 I 0 . 0 0 t - ' ' . ' • I ' ' I ' I I I I - - - , I • I I I I • o p i 0 2 9 0 . 4 3 0 . 8 4 , 1 . 1 5 4 0 3~8,4.19 1 4 . 6 5 s.~tB: + - - - ! - + I , ~~~ F r e q ( R a d / s ) F i g 4 . 9 G a i n v s f r e q p l o t f o r t h e P a n m o v e m e n t - 3 d B g a i n o c c u r s a t 4 . 6 r a d / s ( i e : 0 . 7 3 H z ) I =-~-~~ni~~)l I e : A v a i l a b l e b a n d w i d t h a n d r e q u i r e d b a n d w i t h i s c o m p a r a b l e f o r s a t i s f a c t o r y o p e r a t i o n . 4 . 2 D e s i g n o f E l e c t r o n i c H a r d w a r e 4 . 2 . 1 O v e r v i e w o f t h e C o n t r o l A r c h i t e c t u r e T h e c o n t r o l l e r i s a P I D t y p e , w h e r e t h e s e t p o i n t i s t h e p a n a n g l e i n p u t a n d t h e o u t p u t i s t h e a c t u a l a n g l e o f t h e m e c h a n i s m . + -Sctponn~ L E r r o r p K P e ( t ) I K , J e ( r ) d r D K d e ( t ) d - d t F i g 4 . 1 0 P I D C o n t r o l l e r a n d p l a n t O u t p u t - 3 2 B l o c k d i a g r a m o f t h e c o n t r o l a r c h i t e c t u r e ; I ( I I / ' . 1 I i t ! I / 1 : I I " t = - - ' * ' ( _ : _ ' " ~ (r~·l p < t • i l i o m d • l -~ o l l l t ' < l " n ( m T n I l ' I I I F i g 4 . 1 1 ' l ' l l t n u , E o J l H H I U l ' C • J n t U J ; . - t J H i l i J l M'JI~I.)d ! I I I I I I I I I 1 I ! I I I I I I I I ' , 1 I I 1 1 ! k t a n !'1-L'Jl~mi I I I I I I 1 I , I O v e r a l l c o n t r o l a r c h i t e c t u r e o f t h e s y s t e m I ~,,.~~~ H J t t . h : l r m · t t n l l ' C a ! J f ' ' i 1 J I I t l 1 1 1 - . t l · t u r q u c t . : n r t l n t a n d F i g 4 . 1 1 s h o w s t h e o v e r a l l c o n t r o l a r c h i t e c t u r e o f t h e s y s t e m . C o m p o n e n t s i n t h e d e s i g n a r c h i t e c t u r e ; • M i c r o c o n t r o l l e r b a s e d S e n s o r A D C c i r c u i t • M i c r o c o n t r o l l e r b a s e d P I D c o m p e n s a t o r • H - B r i d g e f o r d r i v i n g t h e D C m o t o r • M i c r o c o n t r o l l e r b a s e d I n t e r f a c e P C B f o r c o m m u n i c a t i o n w i t h t a r g e t t r a c k i n g s y s t e m 4 . 2 . 2 M i c r o C o n t r o l l e r B a s e d S e n s o r A D C a n d P I D C o m p e n s a t o r . A n a l o g t o d i g i t a l c o n v e r t e r a n d P I D c o m p e n s a t o r w a s i m p l e m e n t e d o n a s i n g l e m i c r o c o n t r o l l e r ( P I C 1 6 f 8 7 7 A ) t h e P C B w a s d e s i g n e d w i t h a • 5 V v o l t a g e r e g u l a t o r f o r s u p p l y • 8 p i n c o n n e c t o r h e a d e r f o r d a t a p o r t i n p u t • 5 p i n c o n n e c t e r h e a d e r f o r I C S P ( i n c i r c u i t s e r i a l p r o g r a m m i n g ) • L 2 9 8 D m o t o r c o n t r o l d r i v e r ( i n i t i a l d e s i g n ) • 2 p i n h e a d e r c o n n e c t o r f o r s e n s o r i n p u t 3 3 Vl~: 0 . \ T T • • D ! L i I t . I . r j _ I u . . < B > ! 1 ' f 1 1 " 1 ! ' 9 t i C S P 2 M n 2 < . l ~ 1 Iuu•~::r.- ~ p o < I J . w . H I N 1 u IUO: ~~ ~ , . • · - · ; I C S P 1 I X . J P 1 ~ ;: o "0 :~ 1~-o , : ; : g G N O ' ,~ \ e Y . X V S S E N . 4 B L E A E~I».BLE- 8 1C 1 MCLR~'TH 1 / F \ J . U ' A t i C R A 1 : A J ' " R l - 2 . ' . . . . . . . . 2 F \ A l . ' M o i J R A L . ' T O C K J R J . . ! i . ' M - 4 4 F\EJllR~'AU!i RE1"••~·:.u~ f ' £ 2 ° C S # · A W O S C 1 1 C L K N O S G 2 1 X : L K O U T R < " . £ • T 1 0 S O f V : 1 o T 1 Q : ; i t FO:C~·."CCF1 Ff:C!-'S~K f ' t » ' F : ' : . P O f 1 t j 1 ; P E P 1 I N P U T ! { J U T I U P l i T 2 IU~UTl l t . t P L I T . . : . S E N A S E N - B C J < , C ' c u n O U T J C U T - 1 ! 1 . C " P " ' _ _ c - J ~ 3 \ . ( G t > . D I " ! ' • : J C : S P . 1 _ _ I C S P · S ~ t C14 ~~~ C . l - < 0 " ! C ) I G N O F i g 4 . 1 2 s c h e m a t i c d i a g r a m o f P I C 1 6 F 8 7 7 A m i c r o c o n t r o l l e r s u p p o r t P C B u s e d f o r s e n s o r i n p u t s a n d P I D c o m p e n s a t o r F i g 4 . 1 3 M i c r o c o n t r o l l e r P C B p o p u l a t e d w i t h c o m p o n e n t s 3 4 : . . ; t ~ 1'C1 ~ K S 3 ~ ' - - - - K & - 2 N L.....- -~S1 ~ 4 . 2 . 3 H - B r i d g e H - B r i d g e h a s f o l l o w i n g c h a r a c t e r i s t i c s . • P e a k C u r r e n t H a n d l i n g c a p a c i t y 8 A • R e g e n e r a t i v e b r a k i n g m o d e • T w o c o n t r o l i n p u t s I n p u t s R e s u l t o n t h e c o n n e c t e d m o t o r W 1 W 2 0 0 M o t o r i s f r e e w h e e l i n g ( H i g h i m p e d a n c e ) 0 1 T u r n i n g c k > c k w i s e 1 0 T u r n i n g A n t i c l o c k w i s e 1 1 - - - - - - - · - - - - - - - - - - - - - - - - A c t i v e b r e a k i n g ( r e g e n e r a t i v e m o d e ) T a b l e 4 . 1 T r u t h t a b l e f o r H b r i d g e T h e H - B r i d g e i s a c t u a t e d b y a P W M s i g n a l . T h e m o t o r c a n b e o p e r a t e d i n t w o w a y s w i t h P W M s i g n a l . M o d e 1 : M o d e 2 : T h e m o t o r c a n b e s t o p p e d d u r i n g P W M o f f s t a t e ( r e g e n e r a t i v e b r e a k i n g m o d e ) T h e m o t o r c a n b e a l l o w e d t o f r e e w h e e l d u r i n g o f f s t a t e . E a c h m o d e h a s a d v a n t a g e s a n d d i s a d v a n t a g e s . M o d e 2 h a s t h e a d v a n t a g e o f l e s s e l e c t r i c a l s t r e s s e x e r t e d o n o u t p u t s t a g e s h e n c e a l l o w i n g f o r l e s s c o m p l e x e l e c t r o n i c s d e s i g n . B u t i t h a s t h e d i s a d v a n t a g e o f e x c e s s i v e o v e r s h o o t p o s s i b l y l e a d i n g t o i n s t a b i l i t y . M o d e 1 e x e r t s h i g h e l e c t r i c a l s t r e s s ( c u r r e n t s p i k e s ) d u r i n g d i r e c t i o n c h a n g e s a n d s u d d e n s t o p s . B u t d u e t o a c t i v e b r e a k i n g o v e r s h o o t i s l i m i t e d a n d d e m o n s t r a t e s b e t t e r t r a c k i n g p e r f o r m a n c e , h e n c e m o d e 1 i s u s e d i n t h i s p r o j e c t . . . 1 1 ! 1 . 1 • 1 1 1 ! 1 1 ··~ ~ I I F i g 4 . 1 4 ( a ) s c h e m a t i c d i a g r a m o f t h e H - B r i d g e ( S o u r c e : w w w . i k a l o g i c . c o m ) 3 5 F i g 4 . 1 4 ( b ) H - B r i d g e P C B p o p u l a t e d w i t h c o m p o n e n t s ( t w o u n i t s f o r P a n M o v e m e n t a n d t i l t m o v e m e n t ) 4 . 2 . 4 I n t e r f a c e B o a r d I n t e r f a c e b o a r d i s u s e d f o r c o m m u n i c a t i o n w i t h t h e t a r g e t t r a c k i n g s y s t e m . I n t h i s c a s e t h e c o m p u t e r w h i c h i m p l e m e n t s m a c h i n e v i s i o n s o f t w a r e . I t i s b a s e d o n A t m e l 8 9 s 8 2 5 2 m i c r o c o n t r o l l e r . T h i s h a s 4 I / 0 p o r t s . I t r e c e i v e s t h e c o d e d c o m m a n d f r o m t h e T a r g e t t r a c k i n g s y s t e m ( R a d a r ) t h e n i t d e c o d e s t h e c o m p o s i t e c o m m a n d t o d e t e r m i n e t h e j o i n t a n g l e v a r i a b l e s f o r p a n a n d t i l t m o t o r s a n d p l a c e s t h e c o r r e s p o n d i n g v a l u e s o n r e l e v a n t o u t p u t p o r t s ; i . e : p o r t 1 i s u s e d f o r t i l t m o v e m e n t a n d P o r t 2 i s u s e d f o r p a n m o v e m e n t . 4 . 2 . 5 M a t h e m a t i c a l M o d e l i n g o f t h e S y s t e m U s i n g e q u a t i o n s o b t a i n e d i n c h a p t e r 2 f o r t h e m o t o r / l o a d m o d e l ; r = r r r a 0 1 0 ( ) = - e a 1 7 ( ) = ] _ ( ) a 1 7 ( 4 . 1 ) ( 4 . 2 ) ( 4 . 3 ) 3 6 L£ .- J}e Jfa oe jfJ "1 []+ [= u ()() F o l l o w i n g o p e n l o o p s i m u l i n k m o d e l i s c o n s t r u c t e d u s i n g a b o v e e q u a t i o n s . - K e : L a v c :=~::.ly D s p l a c e m e 1 : F i g 4 . 1 5 S i m u l i n k m o d e l o f t h e c o m p l e t e s y s t e m T h i s s i m u l i n k m o d e l ( f i g 4 . 1 5 ) i s u s e d t o o b t a i n t h e t r a n s f e r f u n c t i o n o f t h e o p e n l o o p s y s t e m ( s e e s e c t i o n 4 . 1 . 4 ) 4 . 2 . 6 E s t i m a t i o n o f V i s c o u s F r i c t i o n o f t h e M o t o r L o a d M o d e l . I n o r d e r t o e s t i m a t e t h e v i s c o u s f r i c t i o n o f t h e s y s t e m , a n e x p e r i m e n t a l m e t h o d i s u s e d . O u t p u t r e s p o n s e o f t h e a c t u a l m o d e l a n d t h e s i m u l i n k m o d e l i s c o m p a r e d a n d t h e v i s c o u s f r i c t i o n o f t h e s i m u l i n k m o d e l i s a d j u s t e d u n t i l t h e o u t p u t s t e p r e s p o n s e o f b o t h s y s t e m s a r e i d e n t i c a l 3 8 I ~I S t e p l _ _ _ _Jc~j J E l l L _ L _ _ _ _ _ l L o a d t o r q u e M J t o r l o a d t o r q u I I I - I I :~~ I ~;-'"1 : c - - - - - - - - - - · - - · - - - 1 J ,-~ - - - I t j ( e t a ) 2 \~ - - - - - - - - \ - - 1 ' . - - l ~scope'. y , 6 t l ( e t a t t b \ 1 1 ( e t a l , . , I . , . _ _ _ ~ 9 • I _ . _ G a i n 2 1 - : ! : - l _ _ _ _ _ _ -./~]. . . _ I cc;;.~! 1 ' 1 a 2 K - ; \ . ~ " - . : : : J " 1 l r < : _ \ I a - K B a " R a i L a I J n 1 - - : - , I M o t o r t o r q u e r r - - ' " " l o c i t y I - - ' 1 I _ 1 L_~ I , ~---- I - - - - . ! . J 1 - 1 -~= ---~dJ · ~2:? ~ _ . _ 3 > ~ ~ - - - - - · - L i _ s - L . _ _ I n t e g r a t o r K e 1 / I a L I n t e g r a t o r 1 T I . . _ - K e / L a / ' / 1 -<-:~]~ i ['=-~-+ ' D i s p l a c e m e n t F i g 4 . 1 6 C l o s e d l o o p v e r s i o n o f t h e m o t o r l o a d m o d e l Ba/~ : - - - - , ? , ----<~K-•---• v e l o c i t y ~ ----l~t~~~;~;-; - - - l . _2~ _ _ 1 I s I~ L _ _ _ _ _ v e l O C i t y F i g 4 . 1 6 s h o w s a c l o s e d l o o p v e r s i o n o f t h e m o t o r l o a d m o d e l . I n o r d e r t o e s t i m a t e t h e v i s c o u s f r i c t i o n a n d l u m p a l l n o n l i n e a r p a r a m e t e r s t o g e t h e r , S t e p r e s p o n s e o f t h e a b o v e s y s t e m w a s c o m p a r e d w i t h t h e a c t u a l s t e p r e s p o n s e o f t h e p a n m o v e m e n t m o t o r . T h e n B a ( v i s c o u s f r i c t i o n c o e f f i c i e n t ) o f t h e s y s t e m w a s a d j u s t e d i n s i m u l i n k m o d e l u n t i l b o t h s t e p r e s p o n s e s w e r e i d e n t i c a l . 2 0 0 9 / 1 1 / 0 6 0 ? : 0 3 : 4 6 D I E " " ' " " ' " " ' " " ' " " ' " " ' " " ' " " S t o p p e d C H Z 1 : 1 ~ - - - - - . ; . - . . . . ; . - 0 . 4 5 V / d i v D C 1 0 k H z E d g e C H ! S A u t o 0 . 0 v ~- < n ' " " " . , i " " ' ' " ' ' " " " " " " ' ' " " " ' " ' F i g 4 . 1 7 ( a ) F i g 4 . 7 ( b ) F i g u r e 4 . 1 7 ( a ) : I n p u t s t e p ( b o t t o m t r a c e ) a n d o u t p u t r e s p o n s e ( t o p t r a c e ) o f t h e a c t u a l p a n M e c h a n i s m , R i s e t i m e 3 7 9 m s . ( b ) O u t p u t s t e p r e s p o n s e o f S i m u l i n k m o d e l , R i s e t i m e 3 7 9 m s W i t h a v i s c o u s d a m p i n g f a c t o r o f 0 . 0 0 0 0 6 2 N m s / r a d , t h e s i m u l i n k m o d e l s t e p r e s p o n s e i s i d e n t i c a l t o t h e a c t u a l s t e p r e s p o n s e . 3 9 T h e r e f o r e i t i s r e a s o n a l b l e t o a s s u m e t h a t t h e v i s c o u s d a m p i n g f a c t o r o f t h e m o t o r l o a d s y s t e m i s 0 . 0 0 0 0 6 2 N m s / r a d . T h i s v a l u e i s u s e d o n t h e a b o v e m o d e l ( f i g 4 . 5 ) i n c a l c u l a t i n g t h e t r a n s f e r f u n c t i o n f o r t h e s y s t e m . 4 . 3 L o o p S h a p i n g ( T r a c k i n g R e s p o n s e ) A b o v e m o d e l ( F i g 4 . 5 ) i s s a v e d i n t h e s i m u l i n k e n v i r o n m e n t a n d u s i n g " l i n m o d " c o m m a n d t h e s t a t e s p a c e m o d e l o f t h e a b o v e s i m u l i n k m o d e l i s o b t a i n e d . A s s u c h , t h e s t a t e s p a c e m o d e l i s ; ( - 3 . 6 A = - 6 . 3 B ~ (~) c = ( 1 o ) D = ( o ) U s i n g m a t l a b c o m m a n d ; f n u m c l , d e n c / j = s s 2 t f ( A , B , C , D ) 1 7 3 1 . 1 ) - 1 3 0 3 . 4 T h e v e l o c i t y t r a n s f e r s f u n c t i o n o f t h e s y s t e m ; B = 2 . 2 7 4 x l 0 - 1 4 S + l 7 3 1 S 2 + 1 3 0 7 S + 1 5 7 0 0 ( 4 . 8 ) ( 4 . 9 ) ( 4 . 1 0 ) ( 4 . 1 1 ) ( 4 . 1 2 ) 4 0 S t e p r e s p o n s e o f t h e v e l o c i t y t r a n s f e r f u n c t i o n i s g i v e n b y F i g 4 . 1 8 , s t e p r e s p o n s e s a t u r a t e s a t 0 . 1 1 i n r e s p o n s e t o a s t e p i n p u t o f u n i t y , t h i s i s d u e t o a t t e n u a t i o n o f t h e s y s t e m . T h e d e s i g n a t t e m p t s t o m e a s u r e t h i s a t t e n u a t i o n f r o m t h e b o d e p l o t f i g 4 . 1 9 . c 1 2 ( J J 1 : J G , E l c ( i E < t : c c E O : s ( J J u : : : J " " c O l r o 1 2 c 0 S t e p R e s p o n s e ( J C I ( l 2 2 5 J l l r r e ( s e c ) F i g 4 . 1 8 t h e v e l o c i t ; . s t e p r e s p o n s e o f t h e s y s t e m B o d e D i a g r a m - ' r 5 ~ l ' "~~~-~t~ ~ J ; . · I c c _ : ( J ~ ·;_;,;;~ ·'~·~}' 4 5 0 , ( J J : s ( J J - S O U ) r o . s = D . . 1 - 1 8 0 r 1 0 1 0 1 0 t 1 0 1 0 5 1 0 F r e q u e n c y ( r a d / s e c ) F i g 4 . 1 9 s h o \ \ S t h e b o d e p l o t o f t h e s y s t e m 4 1 l T h e a n g u l a r p o s i t i o n t r a n s f e r f u n c t i o n B = 2 . 2 7 4 x 1 0 - 1 4 S + l 7 3 1 S 3 t l 3 0 7 S 2 + 1 5 7 0 0 S t h e s y s t e m h a s a 3 0 d B ( a p p r o x ) a t t e n u a t i o n a t 0 H z ; ( 4 . 1 3 ) R e q u i r e d b a n d w i d t h i s o b t a i n e d b y s h a p i n g t h e a b o v e b o d e d i a g r a m ( F i g 4 . 1 9 ) F r o m c h a p t e r 2 . 3 , r e q u i r e d b a n d w i d t h i s 1 4 1 . 6 n r a d / s o r 4 4 4 . 2 6 r a d / s ( P a n m o v e m e n t ) A s i m p l e g a i n K i s u s e d i n t h e f o r w a r d p a t h t o g e t t h e r e q u i r e d b a n d w i d t h . F r o m t h e b o d e p l o t i t i s s e e n t h a t t h e g a i n a t 4 4 4 . 2 6 r a d / s i s - 5 0 . 9 d b T o g e t - 3 d b a t 4 4 4 . 2 6 r a d / s 2 0 l o g K - 5 0 . 9 d B = - 3 d B t h e r e f h r e ; K = l 0 4 7 9 i 2 o K ~249 ( 4 . 1 4 ) T h e H - B r i d g e h a s a n a t u r a l g a i n o f a p p r o x i m a t e l y 5 t h e r e f o r e t h e f o r w a r d g a i n r e q u i r e d f r o m t h e c o n t r o l l e r i s : : : : : 5 0 D r a w i n g a g a i n t h e g a i n a n d p h a s e p l o t w i t h a g a i n o f 2 4 9 i n t h e f o r w a r d p a t h , g 1 v e s ; O J : : : : . < l l D . 2 · c : r n r n : 2 ' O i < l l : : : : . < l l < f ) r n . c 0 . . 0 1 " . l 1 0 1 0 ( B o d e D i a g r a m L S y s t e m : G v V O : V o l t a g e t o v e l o c i t y F r e q u e n c y ( r a d / s e c ) : 4 6 3 M a g n i t u d e ( d B ) : - 3 4 • 1 0 1 0~ F r e q u e n c y ( r a d / s e c ) 1 0 : < F i g 4 . 2 0 g a i n a n d p h a s e p l o t w i t h a g a i n o f 2 4 9 i n f o r w a r d p a t h 1 0 4 1 0 W i t h a g a i n o f 2 4 9 i n t h e f o r w a r d p a t h , t h e a p p r o x i m a t e g a i n a t 4 4 4 . 6 r a d / s i s - 3 d b , t h e r e b y g i v i n g t h e s y s t e m r e q u i r e d b a n d w i d t h . 4 2 4 . 4 S t a b i l i t y o f t h e S y s t e m e n 2 ( 1 J D B · c : O l r o : : 2 ' O i ( 1 J D ( 1 J { / ) r o £ C l . ( I c~ I B o d e D r a g r a m F r _ · • • S y s t e m G v V O : V o l t a g e t o v e l o c i t y F r e q u e n c y ( r a d / s e c ) 3 4 6 M a g n i t u d e ( d B ) - 0 6 5 2 - · S y s t e m G v V O : V o l t a g e t o v e l o c r t y F r e q u e n c y ( r a d / s e c ) 2 5 9 P h a s e ( d e g ) · 9 8 6 I F r e q u e n c y ( r a d / s e c ) F i g 4 2 1 P h a s e m a r g i n 8 2 d e g A c c o r d i n g t o F i g 4 . 2 1 P h a s e m a r g i n i s + 8 2 d e g a n d t h e p h a s e a n g l e n e v e r r e a c h e s 1 8 0 , w h i c h m e a n s t h a t t h e s y s t e m i s i n h e r e n t l y s t a b l e f o r a l l f r e q u e n c i e s a n d a l l g a i n s . 4 . 5 S y s t e m w i t h P o s i t i o n F e e d b a c k ~:=51 I I I u 1 1 - S t e p - I I -~LDI J - 1 - t S c o p e ? L l ~-I' s~~ r~CJI I l _ _ _ _ j I M 1 t o r l o a d l o r q u b 1 1 r - 1 I I I r - - - . ! 1 . . L _ I I t L I t 1 1 ( e t a ) 2 1 - - 1 . ~till I ~\'"''')1 A 1 1 ( e t a ) I - - i . , J r - : : : 5 1 L _ S c o p e f \ ~--· , . , - J I Y - ' = 1 ' . \ ~ / - I ~~"' I " ' ' 2 . . . + c ' l G " " ' V i ---~ L _ lr----~ _ _ _ _ _ J I R o i L • 1 1 1 1 ~ell I I T I r · l - I I ' IMoi;\o~"' I -~7~j - - - -·n-~· ~ _ . j ~ I · ·~IIK>L+~~: ~ I I " ' . L _ _ _ / v c - + . 1 ' - · . J l o t e g c o t o r K e t ! l o ~~ ' - - - - l _ _ [ t l K e / L o S o o p e 4 L ----~-- - . - - . - 1 / z \ " /_~s, - - . T I I l v e l o c . r t y . . l · { l J , . 1 l n t e g r a i . o r l - B a l l a . / j ·<:.~~ I n t e g r a t o r ? v e l o c r t y - - -r------IT~ ~ t . [ g S c o p e 3 F i g 4 2 2 s h o w s t h e c l o s e d l o o p s y s t e m w i t h p o s i t i o n f e e d b a c k . t l V E " I o c 1 t Y 4 3 V e l o c i t y a n d P o s i t i o n r e s p o n s e f o r S t e p i n p u t A s t e p i n p u t i s f e d t o t h e c l o s e d l o o p s y s t e m ( w i t h p o s i t i o n f e e d b a c k ) a n d t h e o u t p u t r e s p o n s e o f t h e v e l o c i t y a n d p o s i t i o n i s o b s e r v e d , F i g 4 . 2 3 ( a ) s h o w s t h e v e l o c i t y r e s p o n s e a n d F i g 4 . 2 3 ( b ) s h o w s t h e p o s i t i o n r e s p o n s e . \"tloctt~ t r a d / s ) l 1 • 1 ; 1 i i l i u l l l f o t • l i , J n \ I T ·r~ ~· d e f i n e d v a l u e - - y r - - cmoo~ Va>o~- ~ /~-=--~-=--~--=2:-k~ ~~~ q=-~-·-_-.;[~~==-~}-~ - - - - · - - - - - - - - - - - - - - - ~-=-=~::==-=- _ _ D'""~=~"-_ _ _ _ _ _ - V F i g 4 . 2 4 b l o c k d i a g r a m o f t h e P I D i m p l e m e n t a t i o n F i g 4 . 2 4 : s h o w s t h e b l o c k d i a g r a m o f t h e P I D i m p l e m e n t a t i o n s e e a p p e n d i x B f o r c o m p l e t e r p i c b a s i c p r o s o u r c e c o d e f i k 4 . 7 T r a j e c t o r y p l a n n i n g S i n c e t h e s y s t e m i s f o r m i l i t a r y a p p l i c a t i o n , p e r f o r m a n c e i s p a r a m o u n t i n s u c h a p p l i c a t i o n s e n e r g y e f f i c i e n c y a n d a e s t h e t i c s o f t h e f i n i s h e d p r o d u c t a r e o f l e s s e r p r i o r i t y . D u e t o t h e l o w b a n d w i d t h o f t h e s y s t e m a n d t h e h i g h g e a r r a t i o , s m o o t h n e s s o f m o v e m e n t i s a n y w a y a s s u r e d b u t t h e d e s i g n c a n o v e r l o o k t r a j e c t o r y p l a n n i n g a l g o r i t h m s a s t h e r e q u i r e m e n t i s t o g e t t h e t a r g e t u n d e r s u r v e i l l a n c e a s q u i c k l y a s p o s s i b l e . I n t h i s p r o c e s s t h e I w a s w i l l i n g t o s a c r i f i c e e n e r g y e f f i c i e n c y f o r h i g h p e r f o r m a n c e . A n o t h e r c o n c e r n w i t h m o v e m e n t s w i t h o u t 4 8 t r a j e c t o r y p l a n n i n g i s t h e s t r e s s e x e r t e d o n o u t p u t s t a g e s d u e t o s u d d e n c h a n g e s i n d i r e c t i o n . T o c o u n t e r t h e s e c o n c e r n s , e l e c t r o n i c s a r e d e s i g n e d w i t h l a r g e s a f e t y m a r g i n s . 4 9 C h a p t e r 5 R e s u l t s 5 . 1 R e s p o n s e t o e x t e r n a l t a r g e t i n p u t s T o a s s e s t h e p e r f o r m a n c e o f t h e m e c h a n i s m , i t i s m a d e t o r e s p o n d t o v a r i o u s i n p u t s g e n e r a t e d b y a n e x t e r n a l t a r g e t s i m u l a t o r . T a r g e t i n p u t s a r e g e n e r a t e d b y a r a n d o m n u m b e r g e n e r a t o r a n d f e d t o t h e c o m p u t e r a s j o i n t a n g l e p a r a m e t e r s . . . . T r a c k i n g p e r f o r m a n c e f o r a r b i t r a r y i n p u t s 2 8 5 . 0 1 . 1 I I I I I I : 1 1 1 1 1 1 1 i l l l l l l l l l l l l l l l l l l l l l l l lli ~!l!lllllll!l:lllllll. llllillllllll ! f l m I i 1 1 1 1 1 1 1 1 1 i I l l &! I ~IiI; l l l i l l \ 1 I I , , I 1 1 ~ ~~ T I , ' 7 r ~ : 1 1 1 I 1 ~ I I T 1 I ~Plio ~ I I I I I I I I I ~ 1 8 5 0 i I \ - . ,I • I I I . . I - - p o s i t i o n i n p u t § . ~ ! I I ! I I I I f t . . I I I I I . J i l l I ~~ I I I + = I j i l l ' t t " ' ' 1'1~ 11 111'1 1 I . - - a c t u a l p o s i t i o n · - I I 1 : g I I I I l ' I I ' I I I I I I I c . 1 I I I I I I I : I ] : I I : I I : I ~ It ' w . U i n n J / 1 1 1 1 1 ~ 1 1 1 1 1 1 1 1 : ~ I I : I l l ~· I ! ~: I l l : I : I f 8 5 . 0 ~ ~ I i \ I ! ~ · I ; IIIII~ 1 ! r I l l : I I l l f f l : 1 i \II~ o ~ N n ~ ~ m ~ r o m o ~ N n ~ ~ m ~ r o m d d d d d d d d d ~ ~ ~ . . . . T i m e ( s e c o n d s ) F i g S . ! r e s p o n s e t o a r b i t r a r y e x t e r n a l t a r g e t i n p u t s F i g 5 . 1 S h o w s t a r g e t t r a c k i n g p e r f o r m a n c e o f t h e p a n M e c h a n i s m . C h a n n e l 4 ( B l u e t r a c e ) i s t h e p o s i t i o n i n p u t , C h a n n e l 3 ( p u r p l e t r a c e ) i s t h e t r a c k i n g o u t p u t . A s c a n b e s e e n t h e r e i s g o o d t r a c k i n g p e r f o r m a n c e b u t t h e m e c h a n i s m l a g s t h e i n p u t . 5 0 5 . 2 R e s p o n s e t o E x t e r n a l S i n u s o i d a l I n p u t s R e s p o n s e f o r 2 7 0 m H z i n p u t 3 0 0 . 0 2 5 0 . 0 J ( / ) C l ) ~ 2 0 0 . 0 C ) C l ) " 0 ·= 1 5 0 . 0 c - a c t u a l p o s i t i o n - i n p u t p o s i t i o n 0 ~ 1 0 0 . 0 0 I 0 . I 5 0 . 0 I I 0 . 0 I I I I I I I 0 - . ; : t c o ( " I ) 1 ' - . . . . . L O e n - . ; : t c o N c . o 0 0 0 . . . . . . . . . . N N N ( " I ) ( " I ) - . ; : t - . ; : t t i m e F i g 5 . 2 ( a ) : i n p u t s i n u s o i d a l s i g n a l o f 2 7 0 m H z , A t a n i n p u t f r e q u e n c y o f 2 7 0 m H z , t h e l a g i s b a r e l y n o t i c e a b l e a n d t h e p o s i t i o n f o l l o w s t h e i n p u t p o s i t i o n i n p u t f a i t h f u l l y . 5 1 3 0 0 . 0 2 5 0 . 0 I l l C l ) ~ 2 0 0 . 0 C l C l ) " C · = 1 5 0 . 0 s : : : : 0 : ; ; · u ; 1 0 0 . 0 0 c . 5 0 . 0 0 . 0 3 0 0 . 0 2 5 0 . 0 I l l C l ) ~ 2 0 0 . 0 C l C l ) " C · = 1 5 0 . 0 s : : : : 0 ~ 1 0 0 . 0 0 c . 5 0 . 0 0 . 0 R e s p o n s e f o r 3 2 9 m H z i n p u t 0 " < t c o ~ " ' : . . . . . - L O m ~ c o N < 0 c ) c ) c ) . . . . . - . . . . . - N N N ( < ) ( < ) . . _ , : . . _ , : t i m e F i g 5 . 2 ( b ) I n p u t s i g n a l o f 3 2 9 m H z R e s p o n s e f o r 4 3 6 m H z i n p u t - - I I C \ . , \ I ( \ I I \ I \ \ j ~ I \ \ \ . . . . - l,~ I I I 0 " < t C O C " 0 ~ . . . . . - L O m " < t C O N < 0 o o o ~ ~ C " \ i C " \ i N ~ ~ - . ¢ - . ¢ t i m e . . . F i g 5 . 2 ( c ) I n p u t s i g n a l o f 4 3 6 m H z - a c t u a l p o s i t i o n - i n p u t p o s i t i o n J - a c t u a l p o s i t i o n - i n p u t p o s i t i o n W h e n t h e f r e q u e n c y i n p u t i s 3 2 9 m H z o u t p u t r e s p o n s e i s s t i l l g o o d b u t w h e n t h e i n p u t f r e q u e n c y i n c r e a s e s t o 4 3 6 m H z , t h e l a g b e c o m e s a p p a r e n t . 5 2 I ' 3 0 0 . 0 2 5 0 . 0 ! / ) C l ) ~ 2 0 0 . 0 C l C l ) ' 0 = 1 5 0 . 0 c . 0 ; : : ; ' i i j 1 0 0 . 0 0 Q , 5 0 . 0 0 . 0 R e s p o n s e f o r 6 3 1 m H z i n p u t I r------tt~jf\---+----\-+1/+-~J_J [ I . I 0 ' < t c o ~ r - - : . . - l O m " ' : c o " ! ~ c i c i 0 . . - . . - N N N ( ' I ) ( ' I ) ' < t ' < t t i m e F i g 5 . 2 ( d ) I n p u t s i g n a l o f 6 3 1 m H z R e s p o n s e f o r 9 2 0 m H z i n p u t - a c t u a l p o s i t i o n - i n p u t p o s i t i o n 3 0 0 . 0 , - . , - - , - . . . . . . . , - , - . . . . . . . , - - - - , - , - - - . , - . . . . , . - - , - - , - - - , 2 5 0 . 0 ! / ) C l ) ~ 2 0 0 . 0 C l C l ) ' 0 · = 1 5 0 . 0 c : 0 1+-+-+-1-f-1+--~~ [ - a c t u a l p o s i t i o n - i n p u t p o s i t i o n ; : : ; ' i i j 1 0 0 . 0 0 Q , 5 0 . 0 0 . 0 0 ' < t c o ~ r - - : . . - l O m ' < t c o N c . o c i c i c i . . - . . - N N N C " ) C " ) - ¢ - ¢ t i m e F i g 5 . 2 ( e ) I n p u t s i g n a l o f 9 2 0 m H z A b o v e t w o g r a p h s s h o w t h e o u t p u t r e s p o n s e t o i n p u t f r e q u e n c i e s o f 6 3 1 m H z a n d 9 2 0 m H z , a t 6 3 1 m H z t h e r e a p p e a r s t o b e a r e s o n a n c e p e a k , i e : o u t p u t a m p l i t u d e i s L a r g e t h a n t h e i n p u t a m p l i t u d e a n d a t 9 2 0 m H z t i m e l a g i s a p p a r e n t a n d s e v e r e . 5 3 & . - C h a p t e r 6 C o n c l u s i o n s a n d f u r t h e r d e v e l o p m e n t s A v i s i o n b a s e d t a r g e t t r a c k i n g t u r r e t b a s e w a s d e s i g n e d c o n s t r u c t e d a n d L a b o r a t o r y t e s t e d . T e s t i n g r e v e a l e d v e r y s a t i s f a c t o r y p e r f o r m a n c e o f t h e s y s t e m . C h a p t e r 5 a n a l y z e d t h e r e s p o n s e o f t h e f i n a l m e c h a n i s m t o v a r i o u s i n p u t s i n d e t a i l , A s a g e n e r a l r u l e f e e d b a c k l o o p b a n d w i d t h s h o u l d b e i n t h e o r d e r o f 3 t o 4 l a r g e r t h a n t h e s e t p o i n t b a n d " " i d t h . H o w e v e r a c c o r d i n g t o t h e F F T a n a l y s i s w e c o n s i d e r t h e s e t p o i n t b a n d w i d t h t o b e i n t h e r a g e o f I H z . - · S y s t e m m o d e l i n g i n t e n t i o n a l l y d i s r e g a r d s n o n - l i n e a r e f f e c t s s u c h a s g r a v i t y e f f e c t s , c a r i o l e s f o r c e , c o u p l i n g e f f e c t s e t c . W h i l e t h i s i s r e a s o n a b l e w h e n t h e s y s t e m i s s l o w m o v i n g a n d t h e a c c u r a c y o f t a r g e t t r a c k i n g i s l e s s i m p o r t a n t , t h e y c a n n o t b e i g n o r e d i n a h i g h s p e e d , h i g h a c c u r a c y s y s t e m . A m o r e r e a l i s t i c m o d e l i n g o f t h e s y s t e m ( L a g r a n g e m o d e l i n g ) n e e d s t o c a r r i e d o u t f o r t h e l a r g e r s y s t e m . T r a c k i n g r e s p o n s e i s e x t r e m e l y s a t i s f a c t o r y a t v e r y l o w f r e q u e n c y l e v e l s b u t a s t h e f r e q u e n c y i n c r e a s e s , t r a c k i n g r e s p o n s e g e t s d e g r a d e d b o t h i n t e r m s o f a m p l i t u d e a n d d e l a y . S o m e f o r m o f d e l a y i s u n a v o i d a b l e a s a r e s u l t o f t h e b a n d w i d t h o f t h e s y s t e m b u t t h e s e s h o r t c o m i n g s c a n b e m i n i m i z e d b y t h e u s e o f a d v a n c e d c o n t r o l s y s t e m s s u c h a s t e e d f o r w a r d l o o p s a n d p a r t i t i o n e d P D c o n t r o l l e r s , w h i c h h a s a s e r v o b a s e d s e c t i o n a n d a m o d e l b a s e d s e c t i o n i n i t s c o n t r o l a r c h i t e c t u r e . F o r s u c h s y s t e m s h i g h s p e e d o n l i n e c a l c u l a t i o n o f i n p u t s t o t h e d r i v e m o t o r s i s e s s e n t i a l , A s s u c h , u s e o f h i g h s p e e d D S P ' s a r e n e e d e d . T h o u g h , t h i s s y s t e m u s e s a v i s i o n s y s t e m t o r e p l a c e t h e r a d a r s y s t e m , c o m m e r c i a l p r o t o t y p e w i l l h a v e t o m a k e u s e o f r e a l i n p u t s f r o m a p r i m a r y r a d a r s y s t e m . T h i s i s b e c a u s e d a t a f r o m r a d a r s y s t e m a r r i v e m u c h l e s s f r e q u e n t l y a n d t h e s y s t e m w i l l h a v e t o m a k e p r e d i c t i o n s o f t a r g e t p o s i t i o n . B y r e f e r r i n g t o F F T a n a l y s i s o f t h e t a r g e t , i t c a n b e a r g u e d t h a t p o w e r a t I H z i s e x t r e m e l y l o w , t h e r e f o r e f o r a l l p r a c t i c a l p u r p o s e s ( e x c e p t i n e x t r e m e c i r c u m s t a n c e s ) t h e s e t p o i n t b a n d w i d t h ( o r t h e t a r g e t b a n d w i d t h ) i s a r o u n d 0 . 3 H z , i n w h i c h c a s e t h e a v a i l a b l e b a n d w i d t h i s m o r e t h a n s u f f i c i e n t t o t r a c k t a r g e t m o v e m e n K B u t a d e t a i l e d s t u d y i s n e e d e d v a l i d a t e t h i s c o n c l u s i o n . T h e s y s t e m i s i n h e r e n t l y s t a b l e w i t h a p h a s e m a r g i n o f 8 2 d e g , w h i c h m e a n s t h a t t h e g a i n c a n b e f u r t h e r i n c r e a s e d ( a c t u a t o r s p e r m i t t i n g ) w i t h o u t c o m p r o m i s i n g t h e s t a b i l i t y o f t h e s y s t e m . T h e r e f o r e w e c a n d e s i g n t h e g u n t u r r e t s o t h a t i t i s c a p a b l e o f e n g a g i n g t h e t a r g e t a t m u c h c l o s e r a n g e . H o w e v e r , u s e o f h i g h e r t o r q u e a c t u a t o r s m a y l e a d t o l o w e r p h a s e m a r g i n o r e v e n s y s t e m i n s t a b i l i t y . I n w h i c h c a s e P h a s e m a r g i n c a n b e a d j u s t e d u s i n g p h a s e l e a d c o m p e n s a t o r s . T h r o u g h o u t t h e d e s i g n p r o c e s s o f t h i s m e c h a n i s m I e x p e r i e n c e d t h e p a r a l y z i n g l i m i t a t i o n s i m p o s e d b y l e s s t h a n i d e a l s e n s o r s . T h e r e f o r e a n y c o m m e r c i a l i z a t i o n o f t h e a h o v e u n i t n e e d s h i g h e s t q u a l i t y s e n s o r s . H i g h r e s o l u t i o n a b s o l u t e r o t a r y s e n s o r s a r e e s s e n t i a l f o r a c c u r a t e p o s i t i o n i n g . 5 4 < ) n a c o m m e r c i a l v e r s i o n , s t r e s s a n a l y s i s n e e d s t o b e c a r r i e d o u t t o c o u n t e r t h e i m p a c t f p r c e s d u e t o a r t i l l e r y t i r e . I ) u e t o s e n s o r n o i s e , i m p e r f e c t i o n s o f P W M d r i v e r a l g o r i t h m a n d t a r g e t t r a c k i n g s y s t e m n o i s e , t h e r e i s c o n s i d e r a b l e v i b r a t i o n a f t e r t h e t a r g e t p o s i t i o n i s a c h i e v e d . T h e r e f o r e h e t t e r s e n s o r o u t p u t s n e e d s t o b e a c q u i r e d p o s s i b l y w i t h t h e u s e o f d i g i t a l f i l t e r s u c h a s k a l m a n f i l t e r i n g o n b o t h i m a g e a c q u i s i t i o n s y s t e m s a s w e l l a s o n t h e p o s i t i o n s e n s o r o u t p u t s . f u r t h e r d e v e l o p m e n t o f t h e m o d e l w i l l r e q u i r e s i g n i f i c a n t a m o u n t s o f m o n e y . I t w i l l a l s o r e q u i r e a c c e s s t o r e a l d a t a i n p u t s f r o m r a d a r s y s t e m s , w h i c h r e q u i r e s c l e a r a n c e f l · o m g o v e r n m e n t a g e n c i e s s u c h a s t h e m i n i s t r y o f d e f e n s e . A d e t a i l e d p r o p o s a l w i l l b e m a d e t o t h e m i n i s t r y o f d e f e n s e w i t h t h e i n t e n t i o n o f o b t a i n i n g f u n d s f o r d e v e l o p i n g a C I H T i m e r c i a l v e r s i o n . 5 5 . . . R e f e r e n c e s : [ I ] S e r - N a m L i m , A h m e d E l a g a m m a l , L a r r y S D a v i s , " l m a g e b a s e d p a n t i l t c a m e r a l c o n t r o l i n a m u l t i c a m e r a s u r v e i l l a n c e e n v i r o n m e n t . " U n i v e r s i t y o f M a r y l a n d , C o l l a g e P a r k , 2 0 0 2 . [ 2 1 A n i r b h a n M u k u r j e e , " D e s i g n a n d D i g i t a l i m p l e m e n t a t i o n o f a P l D c o n t r o l l e r f o r a s i m u l a t e d r o t a r y k n i f e c u t t e r " , M a s t e r T h e s i s , Y o u n g s t o w n s t a t e u n i v e r s i t y , 1 9 9 8 . [ 4 ] D u y N g u y e n - T u o n g , J a n p e t e r s , " L e a r n i n g R o b o t D y n a m i c s f o r C o m p u t e d T o r q u e C o n t r o l u s i n g L o c a l G a u s s i a n P r o c e s s e s R e g r e s s i o n . ' ' M a x p l a n k i n s t i t u t e o f b i o l o g i c a l c y b e r n e t i c s , S p e m a n n s t r a B e 3 8 , 7 2 0 7 6 T u b i n g e n , E C S T S S y m p o s i u m o n L e a r n i n g a n d A d a p t i v e B e h a v i o r s f o r R o b o t i c S y s t e m s : · [ 5 ] h t t p : / / t e l e r o b o t . m e c h . u w a . e d u . a u / R O B O T ( U n i v e r s i t y W e b ) [ 6 ] R . K . M i t t a l a n d L l N a g r a t h , " R o b o t i c s a n d C o n t r o l " , N e w D e l h i . T a t a M c G r a w - H i l L 2 0 0 7 . [ 7 ] B e n j a m i n C K u o , " A u t o m a t i c c o n t r o l s y s t e m s " , N e w D e l h i , P r e n t i c e - H a l L 1 9 9 5 . [ 8 ] S a e e d B . N i k u , " I n t r o d u c t i o n t o r o b o t i c s , A n a l y s y s , S y s t e m s , A p p l i c a t i o n s " , N e w D e l h i , P r e n t i c e - H a l l , 2 0 0 5 . [ l ) ] L J . N a g r a t h a n d M G o p a l , " C o n t r o 1 s y s t e m s E n g i n e e r i n g " , N e w D e l h i , N e w A g e I n t e r n a t i o n a l , 2 0 0 6 . [ 1 0 ] N . M o h a n , T . M . U n d e l a n d , W . P . R o b i n s , " P o w e r E l e c t r o n i c s , C o n v e r t e r s , A p p l i c a t i o n s a n d D e s i g n " , N e w Y o u r k , J o h n w i l e y & S o n s , I n c , 1 9 9 5 [ I I ] T h e o d o r e F . B o g a r t , J r , " E l e c t r o n i c D e v i c e s a n d C i r c u i t s " , N e w D e l h i , U n i v e r s a l b o o k s t a l l , 1 9 9 4 . [ 1 2 ] J o h n D . L e n k , " C i r c u i t E n c y c l o p e d i a a n d T r o u b l e s h o o t i n g G u i d e " , N e w Y o r k , M c G r a w H i l l , 2 0 0 0 . [ 1 3 ] B i s w a N a t h D a t t a , " N u m e r i c a l m e t h o d s f o r l i n e a r c o n t r o l s y s t e m s " , C a l i f o r n i a , E l s i v i e r , 2 0 0 4 . [ 1 4 ] S e t h H u t c h i n s o n , G r e g o r y D . H a g e r , P e t e r L K o r k e , " A T u t o r i a l o n V i s u a l S e r v o C o n t r o l " , I E E E T r a n s a c t i o n s o n R o b o t i c s a n d A u t o m a t i o n , V o l 1 2 , N o S , O c t o b e r 1 9 9 6 . 5 6 z y A p p e n d i x A ( C o o r d i n a t e t r a n s f o r m a t i o n ) ; O ( l i l t ) L l y , x-~ c L : ) X . h y _ , X 0 q 1 e i : : : : : r y S o u r c e : M i t t a l a n d N a g r a t h , " R o b o t i c s a n d C o n t r o l " ( ) p a n = ( ) I R t i l t = e 2 X - L , 5 7 A s s u c h t h e D H t a b l e f o r t h i s r o b o t m a n i p u l a t o r i s a s f o l l o w s I n ca~e t h e e r r o r i s l a r g e r t h a n l D e g r e e t h e p r o g r a m n e e d s t o b r a n c h i n t o a t r a j e c t o r y p l a L i n k ; a ; a ; d ; 8 ; q ; I I 0 0 0 - 2 0 9 0 ( ) 0 e 1 e 1 3 0 - 9 0 0 e z e z . f 0 0 d 4 0 d 4 - - - - _ . - 0 0 1 O T , = I 0 1 0 0 1 0 0 1 0 0 0 0 L C l 0 S 1 0 l I S l 0 - C 1 0 T = 2 0 l 0 0 0 0 ( ) 1 L C 2 ( ) S 2 ( ) " T , = I S 2 ( ) - C 2 0 . ) 0 - - 1 ( ) 0 0 0 0 l - ( ) 0 ( ) 3 T = I ( ) l ( ) 0 4 0 ( ) l d 4 ( ) 0 ( ) 1 O T O T 1 T 2 T 3 T 4 = 1 x 2 x 3 x 4 c l c 2 -S~ _ _ C 1 S 2 - - C I S 2 d 4 + 1 r i i r l 2 r 1 3 r i 4 o I s l c 2 c l - S I S 2 - S 1 S 2 d 4 r 2 1 r n r n r 2 4 T 4 = = s 2 ( ) c 2 C 2 d 4 r 3 1 r 3 2 r o o r 3 4 . ) ) 0 0 ( ) 1 , . 4 1 r 4 2 r 4 3 r 4 4 - C / ) 2 d 4 = r l 4 - 1 - S 1 S 2 d 4 = r 2 4 d 4 C 2 = r 3 4 5 8 . . . 6~ ( £)------------------------( rr.J' ~z.J + z( I _ ~ + )z: UB1 V = If II(} = -c (} ( ) (( r r ) ' t'Z ) und r Z -------------------------I -.J -.J -z um v = e = e A p p e n d i x B ( P I C b a s i c p r o s o u c e c o d e f o r P I D i m p l e m e n t a t i o n ) ' * * * * * * * * * * * * * * * * * * * * * * * * * * E X A M P L E * * * * * * * * * * * * * * * * * * * * * * * * * * ' * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * p a n _ _ t h e t a V A R W O R D ' < - - - T h i s i s m y A c t u a l t h e t a ( a n g l e ) p a n t h e t a r e f V A R W O R D - - - ' < - - - T h i s i s m y d e s i r e d a n g l e p a n _ t h e t a _ r e f _ h i _ c l a m p V A R w o r d p a n _ t h e t a _ r e f _ l o w _ c l a m p V a r w o r d p a n _ D i r e c t i o n v a r P o r t C . O p a n _ D i r e c t i o n _ L a s t V a r b i t a d v a l v a r w o r d a d v a l l v a r w o r d - ' p a n _ p i d _ _ S t a t u s v a r b y t e ' S t a t u s o f t h e P I D - f i l t e r c a n b e r e a d h e r e . p a n _ p i d _ S t a t u s _ O u t _ S a t v a r p a n _ p i d _ S t a t u s . 7 ' F l a g f o r s a t u r a t e d o u t p u t p a n _ _ p i d _ S t a t u s _ I _ S a t v a r p a n _ p i d _ s t a t u s . 6 ' F l a g f o r s a t u r a t e d I n t e g r a l d r i v e . p a n _ p i d _ K p V a r W o r d ' P r o p o r t i o n a l g a i n . p a n _ p i d _ K i V a r W o r d ' I n t e g r a l g a i n . p a n _ p i d _ K d V a r W o r d ' D e r i v a t i v e g a i n . p a n _ p i d _ E r r o r V a r W o r d ' P I D F i l t e r i n p u t . ( s e t p o i n t - a c t u a l ) p a n _ p i d _ O u t V a r W o r d ' P I D F i l t e r o u t p u t ( P + I + D ) p a n _ p i d _ T i v a r B Y T E ' T i m e c o n s t a n t f o r t h e i n t e g r a l t i m e p a n _ p i d _ I _ C l a m p v a r w o r d ' C l a m p f o r t h e i n t e g r a t o r w i n d u p . p a n _ p i d _ O u t _ C l a m p v a r w o r d ' C l a m p t h e f i n a l o u t p u t t o v a l u e i n t h i s v a r . D e f i n e A D C B I T S 1 0 ' S e t n u m b e r o f b i t s i n r e s u l t ' * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ' * * * * * * * * * * * * * * * * * * * P I D - F i l t e r " i n t e r n a l " v a r i a b l e s * * * * * * * * * * * * * * * * * * * * * * * * * * * ' * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * p a n _ p i d _ P V a r W o r d p a n _ p i d _ I V A R W o r d p a n _ p i d _ D V a r W o r d p a n _ p i d _ S i g n V a r B I T p a n _ p i d _ L a s t E r r o r v a r w o r d p a n _ p i d _ E i V a r W o r d p a n _ p i d _ I n t C o u n t V a r B y t e ' P r o p o r t i o n a l d r i v e . ' I n t e g r a l d r i v e . ' D e r i v a t i v e d r i v e . ' K e e p s t r a c k o f t h e s i g n o f v a r i o u s v a l u e s ' L a s t e r r o r , u s e d f o r c a l c u l a t i n g D d r i v e ' I n t e g r a l d r i v e a c c u m u l a t o r . ' C o u n t e r f o r t h e i n t e g r a t o r , m a t c h e s a g a i n s t T i ~************************************************************************* * * * * * , ' * * * * * * * * * * * * * * * * * * * * I n i t i a l i s e v a r i a b l e s * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ~************************************************************************* * * * * * l p a n _ p i d P = 0 I p a n _ p i ( I = 0 p a n _ p i d _ D = 0 ! . . . . J . . ' R e s e t P , I & D v a r i a b l e s . 6 0 i \ D C O N 1 = % 1 0 0 0 0 0 0 0 ' S e t P O R T A a n a l o g a n d r i g h t j u s t i f y r e s u l t ~********************************************************************* ' T h e s e v a r i a b l e s a r e d e c l a r e d b y t h e i n c P I D r o u t i n e b u t ' t h e u s e r n e e d s t o a s s i g n v a l u e s t o t h e m . ' G o s u b G e t A D 2 p a n _ _ p i d _ _ K p = % 1 0 1 0 1 1 1 1 0 0 ' S e t K p t o 7 . 0 o r i g i n a l 2 5 6 p a n _ p i d _ K i = % 1 0 ' S e t K i t o 0 . 5 p a n _ p i d _ K d = % 1 1 0 0 1 0 0 0 ' S e t K d t o 2 . 1 4 o r i g i n a l 1 2 8 p a n _ p i d _ T i = % 1 1 ' U p d a t e I - t e r m e v e r y 3 r d c a l l t o P I D p a n _ _ p i d _ _ I _ C l a m p = % 1 1 0 0 1 0 0 ' C l a m p 1 - t e r m t o m a x ± 1 0 0 p a n _ p i d _ O u t _ C l a m p = % 1 1 1 1 1 1 1 1 ' C l a m p t h e f i n a l o u t p u t t o ± 5 1 1 p a n _ t h e t a _ r e f _ h i _ c l a m p = % 1 1 0 0 1 0 0 0 0 0 p a n _ t h e t a _ r e f _ l o w _ c l a m p = % 1 1 0 0 1 0 T R I S C = 0 S t a r t : G o s u b G e t A D _ p a n ' G e t p o s i t i o n f r o m A t o D - ' p a n _ p i d _ E r r o r = p a n _ t h e t a _ r e f - p a n _ t h e t a ' C a l c u l a t e t h e e r r o r G o s u b P I D _ p a n ' R e s u l t r e t u r n e d i n p i d d r i v e p a n _ D i r e c t i o n = p a n _ p i d _ O u t . 1 5 i f p a n _ D i r e c t i o n < > p a n _ D i r e c t i o n _ L a s t a n d p a n _ p i d _ O u t > 0 T h e n p a n _ D i r e c t i o n _ L a s t = p a n _ D i r e c t i o n e n d i f ' S e t d i r e c t i o n p m a c c o r d n i n g t o s i g n ' p a n _ p i d _ O u t = A B S p a n _ p i d _ O u t f r o m t w o ' s c o m p . t o a b s o l u t e I f p a n _ D i r e c t i o n _ L a s t t h e n H P W M 2 , 2 5 5 , 2 5 0 H P W M 1 , p a n _ p i d _ O u t , 2 5 0 H P W M 2 , 2 5 5 , 2 5 0 E l s e p a n _ p i d _ O u t = - p a n _ p i d _ O u t H P W M 1 , 2 5 5 , 2 5 0 H P W M 2 , p a n _ p i d _ O u t , 2 5 0 e n d i f A D R E S H = O A D R E S L = O a d v a l . h i g h b y t e = 0 a d v a l . l o w b y t e = 0 a d v a l = 0 a d v a l l . h i g h b y t e = 0 a d v a l l . h i g h b y t e = 0 a d v a l l = 0 ' C o n v e r t ' S e t P W M o u t p u t 6 1 ' P a u s e u s 1 p a n _ D i r e c t i o n _ L a s t = p a n _ D i r e c t i o n G o t o S t a r t ' . . . a n d d o i t a g a i n o ' W a i L . . o r i g i n a l 1 0 ' * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * P I D _ p a n : ' T h i s i s t h e e n t r y p o i n t f r o m t h e m a i n a p p o ' * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ' * * * * * * * * * * * * * * * * * * * * * * * P I D F I L T E R * * * * * * * * * * * * * * * * ' * * * * * * * * * * * * * * * * * * ' * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ' C a l c u l a t e t h e p r o p o r t i o n a l d r i v e p a n _ p i d _ P = ( A B S p a n _ p i d _ E r r o r ) * / p a n _ p i d _ K p l f p a n _ p i d _ E r r o r . 1 5 t h e n p a n _ p i d _ P = - p a n _ p i d _ P ' M u l t i p l y b y t h e P - g a i n ' R e - a p p l y s i g n i f p a n _ p i d _ E r r o r i s n e g ' * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ' * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ' C a l c u l a t e t h e I n t e g r a l d r i v e p a n _ p i d _ E i = p a n _ p i d _ E i + p a n _ p i d _ E r r o r ' A d d e r r o r t o a c u m u l a t o r . p a n _ p i d _ I n t C o u n t = p a n _ p i d _ I n t C o u n t + I ' I n c r e m e n t t h e r e s e t - t i m e c o u n t e r . I f p a n _ _ p i d _ I n t C o u n t > = p a n _ p i d _ T i t h e n ' I s i t t i m e t o u p d a t e t h e I - t e r m ? p a n _ p i d _ S i g n = p a n _ p i d _ E i . 1 5 ' S a v e S i g n p a n _ p i d _ E i = A B S p a n _ p i d _ E i ' W o r k w i t h p o s i t i v e n u m b e r s p a n p i d _ E i = p a n _ p i d _ E i * / p a n _ p i d _ K i ' M u l t i p l y b y K i g a i n p a n _ p i d _ E i = p a n _ p i d _ E i I p a n _ p i d _ T i ' D i v i d e b y t h e r e s e t t i m e I f p a n _ p i d _ S i g n t h e n p a n _ p i d _ E i = - p a n _ p i d _ E i ' R e - a p p l y s i g n p a n _ p i d _ I = p a n _ p i d _ l + p a n _ p i d _ E i ' U p d a t e I d r i v e p a n _ p i d _ S i g n = p a n _ p i d _ L 1 5 ' S a v e S i g n p a n _ p i d _ I = A B S p a n _ p i d _ I ' W o r k w i t h p o s i t i v e n u m b e r s i f p a n _ p i d _ I > = p a n _ p i d _ I _ C l a m p t h e n ' I - t e r m i s s a t u r a t e d " " p a n _ p i d _ _ S t a t u s _ l _ S a t = 1 ' s e t p a n _ p i d _ I _ C l a m p f l a g . . . . p a n _ p i d _ I = p a n _ p i d _ I _ C l a m p ' a n d c l a m p I - t e r m s t o w h a t u s e r h a v e s e t . E n d i f I f p a n _ p i d _ S i g n t h e n p a n _ p i d _ I = - p a n _ p i d _ I ' R e - a p p l y s i g n p a n _ p i d _ I n t C o u n t = 0 ' R e s e t t h e c o u n t e r . p a n _ p i d _ E i = 0 ' R e s e t t h e a c c u m u l a t o r . E n d i f l ~************************************************************************* j * * * * * l ! ~************************************************************************* i * * * * * ' \ ' C a l c u l a t e t h e d e r i v a t i v e d r i v e ~ 6 2 1 p a n _ p i d _ _ D = p a n _ p i d _ E r r o r - p a n _ p i d _ L a s t E r r o r p a n p i d _ S i g n = p a n _ p i d _ D . 1 5 ' S a v e S i g n p a n _ p i d _ D = ( A B S p a n _ p i d _ D ) * / p a n _ p i d _ K d ' M u l t i p l y b y K d g a i n I f p a n _ p i d _ S i g n t h e n p a n _ p i d _ D = - p a n _ p i d _ D ' R e - a p p l y s i g n . D e r i v D o n e l : p a n _ p i d _ L a s t E r r o r = p a n _ p i d _ E r r o r ' S t o r e e r r o r f o r n e x t D c a l c . ' * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ' * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ' C a l c u l a t e t h e t o t a l d r i v e . p a n p i d O u t = p a n _ p i d _ P + p a n _ p i d _ I + p a n _ p i d _ D ' C a l c u l a t e i o t a ! d r i v e . . . . p a n _ p i d _ S i g n = p a n _ p i d _ O u t . 1 5 ' S a v e S i g n p a n _ p i d _ O u t = A B S p a n _ p i d _ O u t ' C o n v e r t f r o m t w o ' s c o m p . t o a b s . I f p a n _ p i d _ O u t > = p a n _ p i d _ O u t _ C l a m p t h e n ' O u t p u t i s s a t u r a t e d . . . p a n _ _ p i d _ S t a t u s _ O u t _ S a t = 1 ' s e t s t a t u s b i t a n d . . . p a n _ p i d _ _ o u t = p a n _ p i d _ O u t _ C l a m p ' c l a m p o u t p u t . E n d i f I f p a n _ p i d _ S i g n t h e n p a n _ p i d _ o u t = - p a n _ p i d _ o u t ' R e - a p p l y s i g n . R E T U R N G e t A D _ p a n : A D C O N O = % 1 1 0 0 0 0 0 1 ' C o n f i g u r e a n d t u m o n A I D M o d u l e A N O a s i n p u t p i n 2 l o o p 3 : A D C O N 0 . 2 = 1 ' S t a r t C o n v e r s i o n n o t d o n e 3 : P a u s e u s 1 I f A D C O N 0 . 2 = 1 T h e n n o t d o n e 3 ' W a i t f o r l o w o n b i t - 2 o f A D C O N O , c o n v e r s i o n f i n i s h e d a d v a l . h i g h b y t e = A D R E S H ' M o v e H I G H b y t e o f r e s u l t t o a d v a l a d v a l . l o w b y t e = A D R E S L ' M o v e L O W b y t e o f r e s u l t t o a d v a l a d v a l = a d v a l p a n t h e t a = a d v a l p a n _ t h e t a _ r e f = p o r t d * % 1 1 0 d e c i m a l v a l u e i s 2 5 4 m a x i s 1 0 1 9 . ' M a k e t h e 8 b i t n u m b e r t o 1 0 b i t n u m b e r , m i n i m u m i f p a n _ t h e t a _ r e f > = p a n _ t h e t a _ r e f _ h i _ c l a m p t h e n ' H i p a n _ t h e t a _ r e f = p a n _ t h e t a _ r e f _ h i _ c l a m p e l s e e n d i f e n d i f R e t u r n : ' G e t R e f _ p a n 1 i f p a n _ t h e t a _ r e f < = p a n _ t h e t a _ r e f _ l o w _ c l a m p t h e n p a n _ t h e t a _ r e f = p a n _ t h e t a _ r e f _ l o w _ c l a m p 6 3 . . . . . . - . A p p e n d i x C ( . m f i l e f o r F F T a n a l y s i s ) t = 0 : 0 . 0 0 0 1 : 5 ; A = - 0 . 0 4 3 2 9 6 ; 6 = 3 . 0 6 1 8 ; C = 2 . 0 3 6 1 ; D = - 0 . 2 0 4 8 8 ; E~c0.0()73960; 1 ~ 1 1 j = l : l e n g t h ( t ) y ( I , j ) = A + B * t ( l , j ) + C * t ( l , j ) / \ 2 + D * t ( 1 , j y 3 + E * t ( I , j Y 4 ; ; , : l l d p l o t ( t , y ) ; t i t l e ( \ d r i < l t i P n o l d i s t < m r c \ \ i t h t i t n l ' ( ; \ - 1 0 t l w m k r b t d t ) ' ) ; ; l a b e l ( ' & , t < m c c ( 1 1 1 ) ' ) ; x l a b e l ( ' t i l l h : ( s ) ' ) ; t i g u r e l ! l J j = 1 : l e n g t h ( t ) v ( I j ) = B + 2 * C * t ( I , j ) + 3 * D * t ( l , j Y 2 + 4 * E * t ( 1 J Y 3 ; ; . ' l l d l o t j = 1 : l e n g t h ( t ) a ( l j ) - = 2 * C + 6 * D * t ( I , j ) + 1 2 * E * t ( I , j ) / \ 2 ; l ' l \ ( l s i n t h e t a ( l , 1 ) = 5 / s q r t ( 5 0 ) ; l < l l k = 2 : 1 e n g t h ( y ) s i n t h e t a ( 1 , k ) = 5 / s q r t ( 2 5 + ( 5 - y ( l , k ) ) / \ 2 ) ; t ' l l d t h e t a = ( ( a s i n ( s i n t h e t a ) ) . * 3 6 0 ) / ( 2 * p i ) ; l < n n = 2 : l e n g t h ( y ) - 1 i I t h e t a ( I , n + 1 ) < t h e t a ( 1 , n ) t h e t a ( 1 , n + 1 ) = 1 8 0 - t h e t a ( ! , n + 1 ) ; e b c t h e t a ( I , n + 1 ) = t h e t a ( 1 , n + 1 ) ; e n d e m ! p l o t ( t , t h e t a ) ; t i t l e ( \ d l l d t i o n o f t l l e t < l \ \ i t l l l i t l l l ' ' ) ; y l a b e l ( ' a t l ; , . ' k i 1 1 d q 2 . r e e s ' ) ; x l a b e l ( ' t i l l l l ' i n s e n l l l d s ' ) ; F s = 1 0 0 0 0 ; f i g u r e p l o t ( t , t h e t a ) - · t i t l e ( ' < t l l l : - ' l l l m \ m i " t i o n p r o l i k o f a n ; \ 1 0 t h u n d e r h o l t o h - . , \ . ' l l l ' l l I W I J I < I d i ' - t a l l l ' l ' o f ; ( ) ( ) ( l i n p e r p t : n d i c t t l < u t o t i l l - p < t t h ' ) ; 6 4 f i g u r e % U s e n e x t h i g h e s t p o w e r o f 2 g r e a t e r t h a n o r e q u a l t o l e n g t h ( x ) t o c a l c u l a t e F F T . n f f t = 2 / \ ( n e x t p o w 2 ( l e n g t h ( t h e t a ) ) ) ; % T a k e f f t , p a d d i n g w i t h z e r o s s o t h a t t e n g t h ( f f t x ) i s e q u a l t o n f f t f f t x = f f t ( y , n f f t ) ; % C a l c u l a t e t h e n u m b e r o f u n i q u e p o i n t s N u m U n i q u e P t s = c e i l ( ( n f f t + 1 ) / 2 ) ; % F F T i s s y m m e t r i c , t h r o w a w a y s e c o n d h a l f f f t x = f f t x ( 1 : N u m U n i q u e P t s ) ; % T a k e t h e m a g n i t u d e o f f f t o f x a n d s c a l e t h e f f t s o t h a t i t i s n o t a f u n c t i o n o f % t h e l e n g t h o f x m x = a b s ( f f t x ) / l e n g t h ( t h e t a ) ; % T a k e t h e s q u a r e o f t h e m a g n i t u d e o f f f t o f x . . r m x = r n x . / \ 2 ; % S i n c e w e d r o p p e d h a l f t h e F F T , w e m u l t i p l y m x b y 2 t o k e e p t h e s a m e e n e r g y . % T h e D C c o m p o n e n t a n d N y q u i s t c o m p o n e n t . i f i t e x i s t s , a r e u n i q u e a n d s h o u l d n o t % b e m u l i t p l i e d b y 2 . i f r e m ( n f f t , 2 ) % o d d n f f t e x c l u d e s N y q u i s t p o i n t m x ( 2 : e n d ) = m x ( 2 : e n d ) * 2 ; e l s e m x ( 2 : e n d - 1 ) = m x ( 2 : e n d - 1 ) * 2 ; e n d % f h i s i s a n e v e n l y s p a c e d f r e q u e n c y v e c t o r w i t h N u m U n i q u e P t s p o i n t s . f = ( O : N u m U n i q u e P t s - 1 ) * F s / n f f t ; % G e n e r a t e t h e p l o t , t i t l e a n d l a b e l s . p l o t ( f , m x ) ; t i t l e ( ' P o w e r S p e c t r u m o f A - 1 0 t h u n d e r b o l t ( a n g u l a r v a r i a t i o n ) o b s e r v e d f r o m 3 0 0 0 m ' ) ; x l a b e l ( ' F r e q u e n c y ( H z ) ' ) ; y l a b e l ( ' P o w e r ' ) ; A X I S ( [ O 1 . 5 0 4 0 0 ] ) ; 6 5 A p p e n d i x D ( . m f i l e f o r t r a n s f e r f u n c t i o n e x t r a c t i o n f r o m s i m u l i n k m o d e l ) % g e t t h e s t a t e s p a c e m o d e l o f t h e s i m u l i n k m o d e l p a n t i l t m o d e l [ A , B , C , D ] = l i n m o d ( ' p a n t i l t m o d e l l ' ) % d o t h e s t a t e s p a c e t o t f t r a n s f o r m a t i o n [ n u m c l , d e n c l ] = s s 2 t f ( A , B , C , D ) ; % o b s e r v e t h e s t e p r e s p o n s e s t e p ( n u m c l , d e n c l ) ; G r i d M i n o r f i g u r e ; b o d e ( n u m c l , d e n c l ) ; % V e l o c i t y t r a n s f e r f u n c t i o n G v = t f ( n u m c l , d e n c l ) % d e f i n e t h e t r a n s f e r f u n c t i o n o f s s = t f ( [ l 0 ] , 1 ) ; % p o s i t i o n ( a n g l e ) t r a n s f e r f u n c t i o n G a = G v / s G v . i n p u t N a m e = ' V o l t a g e ' ; G v . O u t p u t N a m e = ' v e l o c i t y ' ; G a . I n p u t N a m e = ' V o l t a g e ' ; G a . O u t p u t N a m e = ' A n g l e ' ; f i g u r e ; s t e p ( G v ) ; G r i d M i n o r f i g u r e ; b o d e ( G v ) ; G r i d M i n o r f i g u r e ; s t e p ( G a ) ; G r i d m i n o r f i g u r e ; b o d e ( G a ) ; G r i d M i n o r J 6 6 A p p e n d i x E ( V B s c r i p p r o g r a m f o r r o b o r e a l m ) i f G e t V a r i a b l e ( " C O G _ X " ) = f a l s e t h e n S e t V a r i a b l e " c o g x " , G e t V a r i a b l e ( " c o g x _ l a s t " ) S e t V a r i a b l e " c o g x " , ( G e t V a r i a b l e ( " C O G _ X " ) - G e t V a r i a b l e ( " I M A G E _ W I D T H " ) / 2 ) S e t V a r i a b l e " c o g x _ l a s t " , G e t V a r i a b l e ( " c o g x " ) e n d i f i f G e t V a r i a b l e ( " C O G _ Y " ) = f a l s e t h e n S e t V a r i a b l e " c o g y " , G e t V a r i a b l e ( " c o g y _ l a s t " ) e l s e S e t V a r i a b l e " c o g y " , ( G e t V a r i a b l e ( " C O G _ Y " ) - G e t V a r i a b l e ( " I M A G E _ H E I G H T " ) / 2 ) S e t V a r i a b l e " c o g y _ l a s t " , G e t V a r i a b l e ( " c o g y " ) e n d i f J I f c u r r e n t i m a g e l o s e s t r a c k i n g r e s o r t t o C O G v a l u e s o f p r e v i o u s f r a m e E l s e c a l c u l a t e C O G c o o r d i n a t e s w i t h r e s p e c t t o C e n t e r o f i m a g e S e t V a r i a b l e " c o g x 2 " , F o r m a t N u m b e r ( G e t V a r i a b l e ( " c o g x " ) * l . l , 1 ) S e t V a r i a b l e " c o g y 2 " , F o r m a t N u m b e r ( G e t V a r i a b l e ( " c o g y " ) * 0 . 5 , 1 ) S e t V a r i a b l e " a n g l e " , F o r m a t N u m b e r ( a t a n 2 ( G e t V a r i a b l e ( " c o g x 2 " ) , - 1 5 0 ) , 1 ) * 5 7 S e t V a r i a b l e " t e s t " , s q r t ( C l n g ( G e t V a r i a b l e ( " c o g x 2 " ) * G e t V a r i a b l e ( " c o g x 2 " ) ) + s q r ( 1 5 0 ) ) S e t V a r i a b l e " a n g l e _ t i l t " , F o r m a t N u m b e r ( a t a n 2 ( G e t V a r i a b l e ( " c o g y " ) , - s q r t ( C l n g ( G e t V a r i a b l e ( " c o g x 2 " ) * G e t V a r i a b l e ( " c o g x 2 " ) ) + s q r ( 1 5 0 ) ) ) , 1 ) * 5 7 C o n v e r t C O G c o o r d i n a t e s o b t a i n e d i n p i x e l v a l u e s t o r e a l v a l u e s i f G e t V a r i a b l e ( " a n g l e " ) < 0 t h e n S e t V a r i a b l e " a n g l e 2 " , 3 6 0 - A b s ( G e t V a r i a b l e ( " a n g l e " ) ) e l s e S e t V a r i a b l e " a n g l e 2 " , G e t V a r i a b l e ( " a n g l e " ) e n d i f i f G e t V a r i a b l e ( " a n g l e 2 " ) < 9 0 t h e n S e t V a r i a b l e " a n g l e 2 " = 9 0 e l s e i f G e t V a r i a b l e ( " a n g l e 2 " ) > 2 7 0 t h e n S e t V a r i a b l e " a n g l e 2 " = 2 7 0 A v o i d n e g a t i v e v a l u e s ' > J a n d s e t u p p e r a n d l o w e r l i m i t s f o r p a n a n g l e e n d i f 6 7 i f G e t V a r i a b l e ( " a n g l e _ t i l t " ) < 0 t h e n S e t V a r i a b l e " a n g l e _ t i l t _ 2 " , 3 6 0 - A b s ( G e t V a r i a b l e ( " a n g l e _ t i l t " ) ) ru~ ~ S e t V a r i a b l e " a n g l e _ t i l t _ 2 " , G e t V a r i a b l e ( " a n g l e _ t i l t " ) e n d i f A v o i d n e g a t i v e v a l u e s a n d s e t u p p e r a n d l o w e r l i m i t s f o r p a n a n g l e S e t V a r i a b l e " a n g l e _ s e r i a l l " , C i n t ( 2 5 5 - ( G e t V a r i a b l e ( " a n g l e 2 " ) 1 1 8 0 * 2 5 5 - 1 2 7 . 5 ) ) S e t V a r i a b l e " a n g l e _ t i l t _ s e r i a l l " , ( C i n t ( ( G e t V a r i a b l e ( " a n g l e _ t i l t _ 2 " ) / 3 6 0 ) * 2 5 5 ) ) S e t V a r i a b l e " a n g l e _ s e r i a l " , G e t V a r i a b l e ( " a n g l e _ s e r i a l l " ) + 3 0 0 S e t V a r i a b l e " a n g l e _ t i l t _ s e r i a l " , G e t V a r i a b l e ( " a n g l e _ t i H _ s e r i a l l " ) + 3 0 0 ~ F o r m a t i n s u i t a b l e f o r m t o b e t r a n s m i t t e d o v e r R s 2 3 2 6 8 A p p e n d i x F ( Q u a d r a t u r e E n c o d e r ) J S O ' U " R N E i Q U A D R A T U R E O U T P U T T A B L E T h i s t a b l e i s i n t e n d e d t o s h o w a v a i l a b l e o u t p u t s a s c u r r e n t l y d e f i n e d . 1 / 4 C Y C L E P E R D E T E N T c w - - - - - - - C b s e d C i r c u n O p e n C i r c u i t C b s e d C i r c u n O p e n C i r c u i t R E C O M M E N D E D I N C R E M E N T A L C O N T R O L D I A G R A M C h a n n e l B I I 7 I . 0 0 15 , 1 1 • 9 /f,~-- - L' _·, _ , , F ' \ '--'==~-"=i-1 S m s DEL A Y I 1 D E B O U N C E I I ( M C 1 4 4 9 0 ) \ L . . . . - r '= ' - - ' = = ' - - . . . . . ! , o . . l ' / ' - - ·" " - = - I 0 J U L _ I l _ _ f L J F U L L C Y C L E P E R D E T E N T ( N o r m a l l y O p e n i n D e t e n t S h o w n ) C W C h a n n e i A O o s e d C i r c u i t O p e n C i r c u i t C l o s e d C i r t u n Ope n C i r c u i t I I I C h a n n e l B ! C U S T O M E R L O G I C C I F C U I T R V i D E C O D E LO G IC D I R E C T I O N M A G N I T U D E U P , t J C M ' H c o u r m R Q u a d r a t u r e E n c o d e r ( C o u r t e s y b o u r n s E l e c t r o n i c s ) 6 9 A p p e n d i x G ( s i m p l e p o t e n t i o m e t e r s ) J 5 1 0 t . 7 : E t N & . S i n g l e t u m p o t e n t i o m e t e r 1 O k ( C o u r t e s y p a n k a j p o t e n t i o m e t e r s ) m u l t i t u m p o t e n t i o m e t e r 1 O k ( C o u r t e s y b o u r n s e l e c t r o n i c s ) A p p e n d i x H ( A b s o l u t e a n a l o g e n c o d e r s c o n t a c t l e s s t y p e ) A b s o l u t e A n a l o g u e S e r v o E n c o d e r ( A 2 S E ) t y p e N C P - 3 0 7 ( c o u r t e s y p a n k a j p o t e n t i o m e t e r s ) 7 0 A p p e n d i x I S i n g l e T u m W i r e W o u n d S e r v o P o t e n t i o m e t e r s , t y p e S S 5 0 ( c o u r t e s y p a n k a j p o t e n t i o m e t e r s ) 7 1