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 tra-

balho, utilizados para simulação do robô, para aquisição de dados e solução da cin-
emá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]
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

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
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 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

35 / / I n v e r s e Ki nem ati cs Problem

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 = −2∗a ∗C ( 1 ) ;
40 e1 = −2∗a ∗C ( 2 ) ;
41 d e l t a ( i ) = e1 ^2+ e2^2−e3 ^ 2 ;
42 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 ) ] ;
43 t h e t a ( i , : ) = 2∗ a t a n ( t ) ;
44 end
45

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

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 ]
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 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

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) {
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

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 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 }