Escolar Documentos
Profissional Documentos
Cultura Documentos
Engenhria Mecânica
Guilerme de Faveri
Aníbal Alexandre Campos Bonilla
2013
69
APÊNDICE A -- Códigos
A.1 SCILAB
O código a seguir é utilizado como simulação do robô, resolvendo a cinemática
inversa para uma trajetória linear entre dois pontos no plano, calculando, monitorando a
matriz Jacobiano e pausando o programa caso esta se torne singular.
1 / / P r o g r a m a que computa a c i n e m á t i c a i n v e r s a do r o b ô 3RRR p l a n a r
2 clear
3 clc
4 clf ;
5 / / I n p u t Data
6 b = 0.08890; / / r o d l e n g h t [m]
7 a = b/2; / / c r a n c k l e n g h t [m]
8 L = 0.26; / / s i d e l e n g h t o f b a s e t r i a n g l e [m]
9 l = 0.08573; / / s i d e l e n g h t o f 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 motors
11
12 h o m e _ p o s i t i o n = [ L / 2 , L∗ s q r t ( 3 ) / 6 , 0 ] ’ ; / / home p o s i t i o n
13 TCP= [ h o m e _ p o s i t i o n ( 1 ) + 0 . 0 5 , h o m e _ p o s i t i o n ( 2 ) , −20] ’ ; / / tool center point
14
19 x c _ f i n a l = TCP ( 1 ) ; / / X p o s i t i o n − f i n a l time
20 y c _ f i n a l = TCP ( 2 ) ; / / Y p o s i t i o n − f i n a l time
21 p h i _ f i n a l = TCP ( 3 ) ; / / end−e 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 m e [ o ]
22 n_passos = 100;
23
24 f o r j =0: n_passos
25 / / Position Analysis
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 _ p a s s o s ;
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 _ p a s s o s ;
28 p h i = ( 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 _ p a s s o s )∗%p i / 1 8 0 ;
29 G = [ xc −( c o s (% p i / 6 + p h i ) ∗ l ∗ ( s q r t ( 3 ) / 3 ) ) , yc −(( l ∗ s q r t ( 3 ) / 3 ) ∗ s i n (% p i / 6 + p h i ) ) ] ;
30 H = [G( 1 ) + l ∗ c o s ( p h i ) , G( 2 ) + l ∗ s i n ( p h i ) ] ;
31 I = [G( 1 ) + l ∗ c o s ( p h i+%p i / 3 ) , G( 2 ) + l ∗ s i n ( p h i+%p i / 3 ) ] ;
32 B = [G ; H ; I ] ;
33 TCP = [ xc , yc ] ;
34
46 / / Jacobian
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 ∗ c o s ( 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 ∗ c o s ( 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 = [A−D ] ;
52 b _ j = [D−B ] ;
53 f o r k =1:3
54 e _ j ( k , : ) = [ TCP−B ( k , : ) ] ;
55 end
56
57 J x = [ 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 J q ( k , k ) = [ ( a _ j ( k , 1 ) ∗ b _ j ( k , 2 )−a _ j ( k , 2 ) ∗ b _ j ( k , 1 ) ) ] ;
63 end
64
65 i f a b s ( r e a l ( J q ( 1 , 1 ) ) ) <0.0001 t h e n b r e a k
66 end
67
68 i f a b s ( r e a l ( J q ( 2 , 2 ) ) ) <0.0001 t h e n b r e a k
69 end
70
71 i f a b s ( r e a l ( J q ( 3 , 3 ) ) ) <0.0001 t h e n b r e a k
72 end
73 J = inv ( 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 ∗ c o s ( 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 ∗ c o s ( 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 ∗ c o s ( 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 ∗ c o s ( 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 código a seguir apresenta o programa escrito no software Processing para captar o
dado do cursor, realizar a conversão entre os espaços de trabalho e calcular a cinemática
inversa.
1 import processing . 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 w i l l be u s i n g
5 i n t ladox =1250;
6 i n t ladoy =710;
7
8 f l o a t k1x = 0 . 0 9 ;
9 f l o a t k2x = 6 . 1 0 7 ∗ pow ( 1 0 , −5 ) ;
10 f l o a t k3x = 0 ;
11 f l o a t k4x = 2 . 3 8 2 ∗ pow ( 1 0 , −2 2 ) ;
12 f l o a t k5x = 4 . 1 0 8 ∗ pow ( 1 0 , −2 3 ) ;
13 f l o a t k6x = 6 2 5 9 4 7 4 . 8 ∗ pow ( 1 0 , −1 4 ) ;
14 f l o a t k7x = 1 . 5 5 1 ∗ pow ( 1 0 , −2 5 ) ;
15 f l o a t k8x = −9556.45∗pow ( 1 0 , −1 4 ) ;
16 f l o a t k9x = −2.019∗pow ( 1 0 , −2 8 ) ;
17
18 f l o a t k1y = 0 . 0 5 ;
19 f l o a t k2y = −0.0000153;
20 f l o a t k3y = 0 . 0 0 0 1 0 4 9 ;
21 f l o a t k4y = −0.0000002;
22 f l o a t k5y = 1 . 1 6 5 ∗ pow ( 1 0 , −8 ) ;
23 f l o a t k6y = −1.956∗pow ( 1 0 , −8 ) ;
24 f l o a t k7y = 1 . 5 6 5 ∗ pow ( 1 0 , −1 0 ) ;
25 f l o a t k8y = 3 . 4 6 4 ∗ pow ( 1 0 , −1 0 ) ;
26 f l o a t k9y = −2.644∗pow ( 1 0 , −1 3 ) ;
27
28 void setup ( )
29 {
A.2 Processing 72
30 s i z e ( ladox , ladoy ) ;
31 frameRate (100) ;
32 println ( Serial . 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 ] , 1 9 2 0 0 ) ;
34 }
35 v o i d draw ( )
36 {
37 f i l l (175) ;
38 r e c t ( 0 , 0 , ladox , ladoy ) ;
39 f i l l ( 2 5 5 , 0 , 0 ) ; / / r g b v a l u e s o RED
40 r e c t ( l a d o x / 2 , l a d o y / 2 , mouseX−l a d o x / 2 , 1 0 ) ; / / xpos , ypos , w i d t h , h e i g h t
41 f i l l ( 0 , 2 5 5 , 0 ) ; / / and GREEN
42 r e c t ( l a d o x / 2 , l a d o y / 2 , 1 0 , mouseY−l a d o y / 2 ) ;
43 u p d a t e ( mouseX , mouseY ) ;
44 }
45 void update ( i n t x , i n t y )
46 {
47
48 i n t P1 = 9 0 ;
49 i n t P2 = 9 0 ;
50 i n t P3 = 9 0 ;
51
52 f l o a t y2= l a d o y −y ;
53 x p o s = k1x+k2x ∗ x+k3x ∗ y2+k4x ∗x∗ y2+k5x ∗pow ( x , 2 ) +k6x ∗pow ( y2 , 2 ) +k7x ∗pow ( x , 2 ) ∗ y2+k8x ∗pow ( y2 , 2 ) ∗x+
k9x ∗pow ( x , 2 ) ∗pow ( y2 , 2 ) ;
54 y p o s = k1y+k2y ∗x+k3y ∗ y2+k4y ∗x∗ y2+k5y ∗pow ( x , 2 ) +k6y ∗pow ( y2 , 2 ) +k7y ∗pow ( x , 2 ) ∗ y2+k8y ∗pow ( y2 , 2 ) ∗x+
k9y ∗pow ( 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 motor a x i s 1 [ m e t e r s ]
59 f l o a t A2x = 0 . 2 6 ; / / p o s i t i o n x of motor a x i s 2 [ m e t e r s ]
60 f l o a t A3x = 0 . 1 3 ; / / p o s i t i o n x of motor a x i s 3 [ m e t e r s ]
61
62 f l o a t A1y = 0 ; / / p o s i t i o n y of motor a x i s 1 [ m e t e r s ]
63 f l o a t A2y = 0 ; / / p o s i t i o n y of motor a x i s 2 [ m e t e r s ]
64 f l o a t A3y = 0 . 1 3 ∗ s q r t ( 3 ) ; / / p o s i t i o n y of motor a x i s 3 [ m e t e r s ]
65
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 b e t w e e n j o i n t [ m e t e r s ]
70
71 f l o a t C1x = x p o s − l ∗ ( c o s ( ( P I / 6 ) + p h i ) ) ∗ ( s q r t ( 3 ) / 3 ) ;
72 f l o a t C2x = C1x + l ∗ ( c o s ( p h i ) ) ;
73 f l o a t C3x = C1x + l ∗ ( c o s ( p h i + ( P I / 3 ) ) ) ;
74
75 f l o a t C1y = y p o s − l ∗ ( s i n ( ( P I / 6 ) + p h i ) ) ∗ ( s q r t ( 3 ) / 3 ) ;
76 f l o a t C2y = C1y + l ∗ ( s i n ( p h i ) ) ;
77 f l o a t C3y = C1y + l ∗ ( s i n ( p h i + ( P I / 3 ) ) ) ;
A.2 Processing 73
78
109 f l o a t t h e t a 1 = 2∗ a t a n ( t 1 ) ∗ 1 8 0 / P I ;
110 f l o a t t h e t a 2 = 2∗ a t a n ( t 2 ) ∗ 1 8 0 / P I ;
111 f l o a t t h e t a 3 = 2∗ a t a n ( t 3 ) ∗ 1 8 0 / P I ;
112
115 i f ( t h e t a 3 <0) {
116 t e t a 3 = t h e t a 3 + 180 +4 5;
117 } else {
118 t e t a 3 = t h e t a 3 −135;
119 }
120
121 i f ( d e l t a 1 <0) {
122 P1 = P1 ;
123 } else {
124 P1 = i n t ( t h e t a 1 ) ;
125 }
126
127 i f ( d e l t a 2 <0) {
A.3 Arduino 74
128 P2 = P2 ;
129 } else {
130 P2 = i n t ( t h e t a 2 ) ;
131 }
132
133 i f ( d e l t a 3 <0) {
134 P3 = P3 ;
135 } else {
136 P3 = i n t ( t e t a 3 ) ;
137 }
138
A.3 ARDUINO
O código 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 < S e r v o . h>
2 Servo servo1 ; Servo servo2 ; Servo servo3 ;
3 // set i n i t i a l v a l u e s f o r x and y
4 i n t ypos = 0 ;
5 i n t xpos= 0 ;
6 void setup ( ) {
7 servo1 . a t t a c h (9) ;
8 servo2 . a t t a c h (6) ;
9 servo3 . a t t a c h (5) ;
10 S e r i a l . b e g i n ( 1 9 2 0 0 ) ; / / 19200 i s t h e r a t e o f c o m m u n i c a t i o n
11 Serial . println ( " Rolling " ) ;
12 }
13 void loop ( ) {
14 s t a t i c i n t v = 0 ; / / v a l u e t o be s e n t t o t h e s e r v o (0 −180)
15 if ( Serial . available () ) {
16 c h a 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 h e s e r i a l p o r t and a s s i g n t o ch
17 s w i t c h ( ch ) { / / s w i t c h b a s e d on t h e v a l u e o f ch
18 case ’0 ’ . . . ’9 ’ : / / if i t ’ s numeric
19 v = v ∗10 + ch − ’ 0 ’ ;
20
21 break ;
22 case ’p ’ :
23 servo1 . write ( v ) ;
24 v = 0;
25 break ;
26 case ’q ’ :
27 servo2 . write ( v ) ;
28 v = 0;
A.3 Arduino 75
29 break ;
30 case ’ r ’ :
31 servo3 . write ( v ) ;
32 v =0;
33 }
34 }
35 }