Software para Atuaao do Prottipo do Rob Paralelo

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 }