3RRR desenvolvido nas Linguagens Nativas de Scilab,
Processing e Arduino Engenria !ec"nica #entro de #i$ncias %ecnolgicas &niversidade do Estado de Santa #atarina 'uiler(e de )averi An*bal Ale+andre #a(pos ,onilla -./3 69 APNDICE A -- Cdigos Neste anexo so aprensentados os cdigos implementados no decorrer deste tra- balho, utilizados para simulao do rob, para aquisio de dados e soluo da cin- emtica inversa, e para comunicao via porta serial com o Arduino. A.1 SCILAB O cdigo a seguir utilizado como simulao do rob, resolvendo a cinemtica inversa para uma trajetria linear entre dois pontos no plano, calculando, monitorando a matriz Jacobiano e pausando o programa caso esta se torne singular. 1 / / Pr ogr ama que comput a a c i n e m t i c a i n v e r s a do r ob 3RRR p l a n a r 2 c l e a r 3 c l c 4 c l f ; 5 / / I nput Dat a 6 b = 0. 08890; / / r od l e n g h t [m] 7 a = b / 2 ; / / cr a nc k l e n g h t [m] 8 L = 0 . 2 6 ; / / s i d e l e n g h t of bas e t r i a n g l e [m] 9 l = 0. 08573; / / s i d e l e n g h t of end e f f e c t o r t r i a n g l e [m] 10 A = [ 0 , 0; L, 0; L/ 2 , L s q r t ( 3 ) / 2 ] ; / / c o o r d i n a t e s of mot or s 11 12 home_pos i t i on = [ L/ 2 , L s q r t ( 3 ) / 6 , 0] ; / / home p o s i t i o n 13 TCP= [ home_pos i t i on ( 1 ) +0. 05 , home_pos i t i on ( 2 ) , 20] ; / / t o o l c e n t e r p o i n t 14 15 x c _ i n i c i a l = home_pos i t i on ( 1 ) ; / / X p o s i t i o n i n i t i a l t i me 16 y c _ i n i c i a l = home_pos i t i on ( 2 ) ; / / Y p o s i t i o n i n i t i a l t i me 17 p h i _ i n i c i a l = home_pos i t i on ( 3 ) ; / / Or i e n t a t i o n i n i t i a l t i me 18 19 x c _ f i n a l = TCP( 1 ) ; / / X p o s i t i o n f i n a l t i me 20 y c _ f i n a l = TCP( 2 ) ; / / Y p o s i t i o n f i n a l t i me 21 p h i _ f i n a l = TCP( 3 ) ; / / ende f f e c t o r o r i e n t a t i o n f i n a l t i me [ o ] 22 n_pas s os = 100; 23 24 f o r j =0: n_pas s os 25 / / P o s i t i o n Ana l ys i s 26 xc = x c _ i n i c i a l + j ( x c _ f i n a l x c _ i n i c i a l ) / n_pas s os ; A.1 Scilab 70 27 yc = y c _ i n i c i a l + j ( y c _ f i n a l y c _ i n i c i a l ) / n_pas s os ; 28 phi = ( p h i _ i n i c i a l + j ( p h i _ f i n a l p h i _ i n i c i a l ) / n_pas s os )%pi / 1 8 0 ; 29 G = [ xc ( cos (%pi / 6+ phi ) l ( s q r t ( 3 ) / 3 ) ) , yc (( l s q r t ( 3 ) / 3 ) s i n (%pi / 6+ phi ) ) ] ; 30 H = [G( 1 ) + l cos ( phi ) , G( 2 ) + l s i n ( phi ) ] ; 31 I = [G( 1 ) + l cos ( phi+%pi / 3 ) , G( 2 ) + l s i n ( phi+%pi / 3 ) ] ; 32 B = [G; H; I ] ; 33 TCP = [ xc , yc ] ; 34 35 / / I n v e r s e Ki nemat i cs Pr obl em 36 f o r i =1: 3 37 C = [ ( B( i , 1 )A( i , 1 ) ) , ( B( i , 2 )A( i , 2 ) ) ] ; 38 e3 = C( 1 ) ^2 + C( 2 ) ^2 + a ^2 b ^2; 39 e2 = 2aC( 1 ) ; 40 e1 = 2aC( 2 ) ; 41 d e l t a ( i ) = e1 ^2+e2^2e3 ^2; 42 t = [( e1+ s q r t ( d e l t a ( i ) ) ) / ( e3e2 ) , (e1s q r t ( d e l t a ( i ) ) ) / ( e3e2 ) ] ; 43 t h e t a ( i , : ) = 2 a t a n ( t ) ; 44 end 45 46 / / J a c obi a n 47 D = [ a cos ( t h e t a ( 1 , 1 ) ) , a s i n ( t h e t a ( 1 , 1 ) ) ; 48 (A( 2 , 1 ) +a cos ( t h e t a ( 2 , 1 ) ) ) , (A( 2 , 2 ) +a s i n ( t h e t a ( 2 , 1 ) ) ) ; 49 (A( 3 , 1 ) +a cos ( t h e t a ( 3 , 1 ) ) ) , (A( 3 , 2 ) +a s i n ( t h e t a ( 3 , 1 ) ) ) ] 50 51 a _j = [AD] ; 52 b_j = [DB] ; 53 f o r k =1: 3 54 e _j ( k , : ) = [ TCPB( k , : ) ] ; 55 end 56 57 Jx = [ b_j ( 1 , 1 ) , b_j ( 1 , 2 ) , ( e _j ( 1 , 1 ) b_j ( 1 , 2 )e _j ( 1 , 2 ) b_j ( 1 , 1 ) ) ; 58 b_j ( 2 , 1 ) , b_j ( 2 , 2 ) , ( e _j ( 2 , 1 ) b_j ( 2 , 2 )e _j ( 2 , 2 ) b_j ( 2 , 1 ) ) ; 59 b_j ( 3 , 1 ) , b_j ( 3 , 2 ) , ( e _j ( 3 , 1 ) b_j ( 3 , 2 )e _j ( 3 , 2 ) b_j ( 3 , 1 ) ) ] 60 61 f o r k =1: 3 62 Jq ( k , k ) = [ ( a _j ( k , 1 ) b_j ( k , 2 )a _j ( k , 2 ) b_j ( k , 1 ) ) ] ; 63 end 64 65 i f abs ( r e a l ( Jq ( 1 , 1 ) ) ) <0. 0001 t he n br eak 66 end 67 68 i f abs ( r e a l ( Jq ( 2 , 2 ) ) ) <0. 0001 t he n br eak 69 end 70 71 i f abs ( r e a l ( Jq ( 3 , 3 ) ) ) <0. 0001 t he n br eak 72 end 73 J = i nv ( Jq ) Jx 74 75 M = [A( 1 , 1 ) A( 1 , 2 ) ; 76 a cos ( t h e t a ( 1 , 1 ) ) , a s i n ( t h e t a ( 1 , 1 ) ) ; A.2 Processing 71 77 B( 1 , 1 ) , B( 1 , 2 ) ; 78 B( 2 , 1 ) , B( 2 , 2 ) ; 79 (A( 2 , 1 ) +a cos ( t h e t a ( 2 , 2 ) ) ) , (A( 2 , 2 ) +a s i n ( t h e t a ( 2 , 2 ) ) ) ; 80 A( 2 , 1 ) ,A( 2 , 2 ) ; 81 (A( 2 , 1 ) +a cos ( t h e t a ( 2 , 2 ) ) ) , (A( 2 , 2 ) +a s i n ( t h e t a ( 2 , 2 ) ) ) ; 82 B( 2 , 1 ) , B( 2 , 2 ) ; 83 B( 3 , 1 ) , B( 3 , 2 ) ; 84 (A( 3 , 1 ) +a cos ( t h e t a ( 3 , 2 ) ) ) , (A( 3 , 2 ) +a s i n ( t h e t a ( 3 , 2 ) ) ) ; 85 A( 3 , 1 ) ,A( 3 , 2 ) ; 86 (A( 3 , 1 ) +a cos ( t h e t a ( 3 , 2 ) ) ) , (A( 3 , 2 ) +a s i n ( t h e t a ( 3 , 2 ) ) ) ; 87 B( 3 , 1 ) , B( 3 , 2 ) 88 B( 1 , 1 ) , B( 1 , 2 ) ; ] ; A.2 PROCESSING O cdigo a seguir apresenta o programa escrito no software Processing para captar o dado do cursor, realizar a converso entre os espaos de trabalho e calcular a cinemtica inversa. 1 i mpor t p r o c e s s i n g . s e r i a l . ; 2 f l o a t xpos =90; 3 f l o a t ypos =90; 4 S e r i a l p o r t ; / / The s e r i a l p o r t we wi l l be us i ng 5 i n t l adox =1250; 6 i n t l adoy =710; 7 8 f l o a t k1x =0. 09; 9 f l o a t k2x =6. 107pow( 10 , 5) ; 10 f l o a t k3x =0; 11 f l o a t k4x =2. 382pow( 10 , 22) ; 12 f l o a t k5x =4. 108pow( 10 , 23) ; 13 f l o a t k6x =6259474. 8pow( 10 , 14) ; 14 f l o a t k7x =1. 551pow( 10 , 25) ; 15 f l o a t k8x=9556. 45pow( 10 , 14) ; 16 f l o a t k9x=2.019pow( 10 , 28) ; 17 18 f l o a t k1y =0. 05; 19 f l o a t k2y = 0. 0000153; 20 f l o a t k3y =0. 0001049; 21 f l o a t k4y = 0. 0000002; 22 f l o a t k5y =1. 165pow( 10 , 8) ; 23 f l o a t k6y=1.956pow( 10 , 8) ; 24 f l o a t k7y =1. 565pow( 10 , 10) ; 25 f l o a t k8y =3. 464pow( 10 , 10) ; 26 f l o a t k9y=2.644pow( 10 , 13) ; 27 28 voi d s e t up ( ) 29 { A.2 Processing 72 30 s i z e ( l adox , l adoy ) ; 31 f r ameRat e ( 100) ; 32 p r i n t l n ( S e r i a l . l i s t ( ) ) ; 33 p o r t = new S e r i a l ( t h i s , S e r i a l . l i s t ( ) [ 0 ] , 19200) ; 34 } 35 voi d draw ( ) 36 { 37 f i l l ( 175) ; 38 r e c t ( 0 , 0 , l adox , l adoy ) ; 39 f i l l ( 255 , 0 , 0) ; / / r gb va l ue so RED 40 r e c t ( l adox / 2 , l adoy / 2 , mouseXl adox / 2 , 10) ; / / xpos , ypos , wi dt h , h e i g h t 41 f i l l ( 0 , 255 , 0) ; / / and GREEN 42 r e c t ( l adox / 2 , l adoy / 2 , 10 , mouseYl adoy / 2 ) ; 43 upda t e ( mouseX , mouseY) ; 44 } 45 voi d upda t e ( i n t x , i n t y ) 46 { 47 48 i n t P1 =90; 49 i n t P2 =90; 50 i n t P3 =90; 51 52 f l o a t y2=l adoyy ; 53 xpos = k1x+k2xx+k3xy2+k4xxy2+k5xpow( x , 2 ) +k6xpow( y2 , 2 ) +k7xpow( x , 2 ) y2+k8xpow( y2 , 2 ) x+ k9xpow( x , 2 ) pow( y2 , 2 ) ; 54 ypos = k1y+k2yx+k3yy2+k4yxy2+k5ypow( x , 2 ) +k6ypow( y2 , 2 ) +k7ypow( x , 2 ) y2+k8ypow( y2 , 2 ) x+ k9ypow( x , 2 ) pow( y2 , 2 ) ; 55 56 i n t phi = 0; 57 58 f l o a t A1x = 0; / / p o s i t i o n x of mot or a x i s 1 [ met er s ] 59 f l o a t A2x = 0 . 2 6 ; / / p o s i t i o n x of mot or a x i s 2 [ met er s ] 60 f l o a t A3x = 0 . 1 3 ; / / p o s i t i o n x of mot or a x i s 3 [ met er s ] 61 62 f l o a t A1y = 0; / / p o s i t i o n y of mot or a x i s 1 [ met er s ] 63 f l o a t A2y = 0; / / p o s i t i o n y of mot or a x i s 2 [ met er s ] 64 f l o a t A3y = 0. 13 s q r t ( 3 ) ; / / p o s i t i o n y of mot or a x i s 3 [ me t er s ] 65 66 f l o a t a = 0. 04445; / / cr ank l e n g t h [ met er s ] 67 f l o a t b = 0. 08890; / / r od l e n g t h [ met e r s ] 68 69 f l o a t l = 0. 08573; / / move p l a t f o r m d i s t a n c e bet ween j o i n t [ met er s ] 70 71 f l o a t C1x = xpos l ( cos ( ( PI / 6 ) +phi ) ) ( s q r t ( 3 ) / 3 ) ; 72 f l o a t C2x = C1x + l ( cos ( phi ) ) ; 73 f l o a t C3x = C1x + l ( cos ( phi + ( PI / 3 ) ) ) ; 74 75 f l o a t C1y = ypos l ( s i n ( ( PI / 6 ) +phi ) ) ( s q r t ( 3 ) / 3 ) ; 76 f l o a t C2y = C1y + l ( s i n ( phi ) ) ; 77 f l o a t C3y = C1y + l ( s i n ( phi +( PI / 3 ) ) ) ; A.2 Processing 73 78 79 f l o a t d1x = C1x A1x ; 80 f l o a t d2x = C2x A2x ; 81 f l o a t d3x = C3x A3x ; 82 83 f l o a t d1y = C1y A1y ; 84 f l o a t d2y = C2y A2y ; 85 f l o a t d3y = C3y A3y ; 86 87 f l o a t e11 = 2ad1y ; 88 f l o a t e21 = 2ad1x ; 89 f l o a t e31 = d1xd1x + d1yd1y + aa bb ; 90 91 f l o a t d e l t a 1 = e11e11 + e21e21 e31e31 ; 92 93 f l o a t e12 = 2ad2y ; 94 f l o a t e22 = 2ad2x ; 95 f l o a t e32 = d2xd2x + d2yd2y + aa bb ; ; 96 97 f l o a t d e l t a 2 = e12e12 + e22e22 e32e32 ; 98 99 f l o a t e13 = 2ad3y ; 100 f l o a t e23 = 2ad3x ; 101 f l o a t e33 = d3xd3x + d3yd3y + aa bb ; ; 102 103 f l o a t d e l t a 3 = e13e13 + e23e23 e33e33 ; 104 105 f l o a t t 1 = (e11+ s q r t ( d e l t a 1 ) ) / ( e31e21 ) ; 106 f l o a t t 2 = (e12s q r t ( d e l t a 2 ) ) / ( e32e22 ) ; 107 f l o a t t 3 = (e13s q r t ( d e l t a 3 ) ) / ( e33e23 ) ; 108 109 f l o a t t h e t a 1 = 2 a t a n ( t 1 ) 180/ PI ; 110 f l o a t t h e t a 2 = 2 a t a n ( t 2 ) 180/ PI ; 111 f l o a t t h e t a 3 = 2 a t a n ( t 3 ) 180/ PI ; 112 113 f l o a t t e t a 3 ; 114 115 i f ( t he t a 3 <0) { 116 t e t a 3 = t h e t a 3 +180+45; 117 } e l s e { 118 t e t a 3 = t he t a 3 135; 119 } 120 121 i f ( de l t a 1 <0) { 122 P1 = P1 ; 123 } e l s e { 124 P1 = i n t ( t h e t a 1 ) ; 125 } 126 127 i f ( de l t a 2 <0) { A.3 Arduino 74 128 P2 = P2 ; 129 } e l s e { 130 P2 = i n t ( t h e t a 2 ) ; 131 } 132 133 i f ( de l t a 3 <0) { 134 P3 = P3 ; 135 } e l s e { 136 P3 = i n t ( t e t a 3 ) ; 137 } 138 139 p o r t . wr i t e ( P1+" p" ) ; 140 p o r t . wr i t e ( P2+" q" ) ; 141 p o r t . wr i t e ( P3+" r " ) ; 142 } A.3 ARDUINO O cdigo a seguir utilizado no Arduino para ler a porta serial e enviar um valor de ngulo entre 0 e 180 graus para os servos. 1 # i n c l u d e <Ser vo . h> 2 Ser vo s er vo1 ; Ser vo s er vo2 ; Ser vo s er vo3 ; 3 / / s e t i n i t i a l va l ue s f o r x and y 4 i n t ypos = 0; 5 i n t xpos = 0; 6 voi d s e t up ( ) { 7 s e r vo1 . a t t a c h ( 9 ) ; 8 s e r vo2 . a t t a c h ( 6 ) ; 9 s e r vo3 . a t t a c h ( 5 ) ; 10 S e r i a l . begi n ( 19200) ; / / 19200 i s t he r a t e of communi cat i on 11 S e r i a l . p r i n t l n ( " Ro l l i ng " ) ; 12 } 13 voi d l oop ( ) { 14 s t a t i c i n t v = 0; / / va l ue t o be s e n t t o t he s e r vo (0 180) 15 i f ( S e r i a l . a v a i l a b l e ( ) ) { 16 c ha r ch = S e r i a l . r e a d ( ) ; / / r e a d i n a c h a r a c t e r from t he s e r i a l p o r t and a s s i g n t o ch 17 s wi t c h ( ch ) { / / s wi t c h bas ed on t he va l ue of ch 18 c a s e 0 . . . 9 : / / i f i t s numer i c 19 v = v10 + ch 0 ; 20 21 br eak ; 22 c a s e p : 23 s e r vo1 . wr i t e ( v ) ; 24 v = 0; 25 br eak ; 26 c a s e q : 27 s e r vo2 . wr i t e ( v ) ; 28 v = 0; A.3 Arduino 75 29 br eak ; 30 c a s e r : 31 s e r vo3 . wr i t e ( v ) ; 32 v =0; 33 } 34 } 35 }