Software para Atuaçao do Protótipo do Robô Paralelo
3RRR desenvolvido nas Linguagens Nativas de Scilab,
Processing e Arduino
Engenhria Mecânica
Centro de Ciëncias Tecnológicas
Universidade do Estado de Santa Catarina
Guilerme de Faveri
Aníbal Alexandre Campos Bonilla
2013
69
APÊNDICE A -- Códigos
Neste anexo são aprensentados os códigos implementados no decorrer deste trabalho, utilizados para simulação do robô, para aquisição de dados e solução da cinemática inversa, e para comunicação via porta serial com o Arduino.
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]
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
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] ’ ;
10
11
/ / tool center point
14
15
x c _ i n i c i a l = home_position (1) ;
/ / X p o s i t i o n − i n i t i a l time
16
y c _ i n i c i a l = home_position (2) ;
/ / Y p o s i t i o n − i n i t i a l time
17
p h i _ i n i c i a l = home_position (3) ;
/ / O r i e n t a t i o n − i n i t i a l time
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;
18
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
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
35
/ / I n v e r s e Ki nem ati cs Problem
36
f o r i =1:3
C = [ ( B ( i , 1 )−A( i , 1 ) ) , ( B ( i , 2 )−A( i , 2 ) ) ] ;
37
38
e3 = C ( 1 ) ^2 + C ( 2 ) ^2 + a ^2 − b ^ 2 ;
39
e2 = −2∗a ∗C ( 1 ) ;
40
e1 = −2∗a ∗C ( 2 ) ;
41
d e l t a ( i ) = e1 ^2+ e2^2−e3 ^ 2 ;
t = [( − e1 + s q r t ( d e l t a ( i ) ) ) / ( e3−e2 ) , (−e1−s q r t ( d e l t a ( i ) ) ) / ( e3−e2 ) ] ;
42
t h e t a ( i , : ) = 2∗ a t a n ( t ) ;
43
44
end
45
46
47
/ / Jacobian
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
J q ( k , k ) = [ ( a _ j ( k , 1 ) ∗ b _ j ( k , 2 )−a _ j ( k , 2 ) ∗ b _ j ( k , 1 ) ) ] ;
62
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
76
M = [A( 1 , 1 ) A( 1 , 2 ) ;
a∗ cos ( t h e t a ( 1 , 1 ) ) , a∗ s i n ( t h e t a ( 1 , 1 ) ) ;
70
A.2 Processing
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 ) ; ] ’ ;
71
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
{
72
A.2 Processing
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+
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+
k9x ∗pow ( x , 2 ) ∗pow ( y2 , 2 ) ;
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
66
f l o a t a = 0.04445;
/ / crank length [ meters ]
67
f l o a t b = 0.08890;
/ / rod l e n g t h [ meters ]
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 ]
68
69
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
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 = −2∗a ∗ d1y ;
88
f l o a t e21 = −2∗a ∗ d1x ;
89
f l o a t e31 = d1x ∗ d1x + d1y ∗ d1y + a ∗ a − b∗ b ;
90
91
f l o a t d e l t a 1 = e11 ∗ e11 + e21 ∗ e21 − e31 ∗ e31 ;
92
93
f l o a t e12 = −2∗a ∗ d2y ;
94
f l o a t e22 = −2∗a ∗ d2x ;
95
f l o a t e32 = d2x ∗ d2x + d2y ∗ d2y + a ∗ a − b∗ b ; ;
96
97
f l o a t d e l t a 2 = e12 ∗ e12 + e22 ∗ e22 − e32 ∗ e32 ;
98
99
f l o a t e13 = −2∗a ∗ d3y ;
100
f l o a t e23 = −2∗a ∗ d3x ;
101
f l o a t e33 = d3x ∗ d3x + d3y ∗ d3y + a ∗ a − b∗ b ; ;
102
103
f l o a t d e l t a 3 = e13 ∗ e13 + e23 ∗ e23 − e33 ∗ e33 ;
104
105
f l o a t t 1 = (− e11 + s q r t ( d e l t a 1 ) ) / ( e31−e21 ) ;
106
f l o a t t 2 = (−e12−s q r t ( d e l t a 2 ) ) / ( e32−e22 ) ;
107
f l o a t t 3 = (−e13−s q r t ( d e l t a 3 ) ) / ( e33−e23 ) ;
108
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
113
float teta3 ;
114
115
i f ( t h e t a 3 <0) {
t e t a 3 = t h e t a 3 + 180 +4 5;
116
117
} else {
t e t a 3 = t h e t a 3 −135;
118
119
}
120
121
i f ( d e l t a 1 <0) {
P1 = P1 ;
122
123
} else {
P1 = i n t ( t h e t a 1 ) ;
124
125
}
126
127
i f ( d e l t a 2 <0) {
73
74
A.3 Arduino
P2 = P2 ;
128
129
} else {
P2 = i n t ( t h e t a 2 ) ;
130
131
}
132
133
i f ( d e l t a 3 <0) {
P3 = P3 ;
134
135
} else {
P3 = i n t ( t e t a 3 ) ;
136
137
}
138
139
p o r t . w r i t e ( P1+ " p " ) ;
140
p o r t . w r i t e ( P2+ " q " ) ;
141
p o r t . w r i t e ( P3+ " r " ) ;
142
}
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
4
i n t ypos = 0 ;
i n i t i a l v a l u e s f o r x and y
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 ’ : / /
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;
if
i t ’ s numeric
A.3 Arduino
29
break ;
30
case ’ r ’ :
31
servo3 . write ( v ) ;
32
v =0;
33
}
34
}
35
}
75