Gabriel Aurélio Cogo

Ítalo César Porto Xavier

João Henrique Charif Lakoski

Projeto Final de Intrumentação Eletrônica - 2ELE045

Modelo 3D virtual controlado pelo sensor MPU-6050 e um Arduino

Data de realização do experimento:

26 de Julho de 2019
Série/Turma:
1000/1012
Prof. Dr. José Alexandre de França

7 de Agosto de 2019
 Resumo
Com o intuito do estudo empírico de sensores aprendidos na disciplina de instrumen-
tação eletrônica, foi aferido dados e também, entendido o comportamento de um sensor
acelerômetro e giroscópio, identificando suas grandezas físicas e o processo da conversão
desses dados para o meio digital. O projeto consiste em um modelamento 3D virtual con-
trolado pelo sensor MPU-6050 com auxílio dos softwares Arduino e Processing 3. Para o
experimento, também foi nessessário uma matriz de contato (protoboard) para as conexões
necessárias.

1
Sumário
Resumo 1

1 Descrição do Problema 3

2 Materiais e Ferramentas Computacionais 3

3 Sensor MPU6050 3

4 Montagem 5

5 Processamento 6
5.1 Biblioteca para o software Processing e IDE Arduino . . . . . . . . . . . . . . . 6

6 Testes 8

Referências 20

2
1 Descrição do Problema
Este projeto objetivou-se a trabalhar com sensores, processamento de dados e simulação dos
dados utilizando, um software para visualização de um objeto pelo computador, um microcon-
trolador para o processamento de dados e um sensor, módulo com um giroscópio e acelerômetro
embutidos.
Para tanto foi necessário escrever linhas de código para obter os valores do sensor a fim de
serem traduzidos pelo software e movimentar o modelo 3D em tempo real.

2 Materiais e Ferramentas Computacionais

Os materiais de hardware para este projeto são listados a baixo.

• Arduino modelo UnoR3

• Módulo para Arduino MPU-6050

• Matriz de Contato (Protoboard )

Foi utilizado os seguintes softwares para a elaboração deste projeto.

• IDE (Arduino)

• Processing 3 (3D)

3 Sensor MPU6050
O placa vista pela figura 1 trata-se do módulo utilizado no projeto.

Figura 1: Módulo MPU6050 com acelerômetro e giroscópio.

O MPU-6050 é um dispositivo integrado de 6 eixos que combina um giroscópio de 3 eixos,

acelerômetro de 3 eixos e um Processador de Movimento Digital (DMP) no módulo visto an-
teriormente. Com seu barramento de sensor I2C, ele aceita diretamente as entradas de uma
bússola externa de 3 eixos para fornecer uma saída completa de 9 eixos. O MPU-60X0 tam-
bém foi projetado para interagir com múltiplos sensores digitais não inerciais, como sensores
de pressão, na sua porta auxiliar I2C.

3
 O MPU-6050 possui três conversores analógico-digitais de 16 bits (ADCs) para digitalizar
as saídas do giroscópio e três ADCs de 16 bits para digitalizar as saídas do acelerômetro.
Para rastreamento preciso de movimentos rápidos e lentos, as peças possuem um giroscópio
programável pelo usuário em escala completa de ±250, ±500, ±1000 e ±2000o /s (dps) e um
acelerômetro programável pelo usuário em escala completa de ±2g, ±4g, ±8g e ±16g.
A figura 2 mostra o sentido do eixo triplo para o giroscópio do MPU-6050.

Figura 2: Orientação dos eixos de sensibilidade e polarização de rotação.

O MPU-6050 possui um multiplexador de interface, que conecta os pinos I2C do processador

do sistema (23-SDA e 24-SCL) diretamente aos pinos de barramento auxiliar I2C do sensor
auxiliar 6 e 7 (AUX DA e AUX CL). "Barramento do processador I2C: para ler todos os dados
do sensor a partir do MPU e para configurar sensores externos."(Ver figura 3).

Figura 3: Barramentos do MPU-6050

4
 Os registros de dados do sensor contêm os dados mais recentes de medição de giroscópio,
acelerômetro, sensor auxiliar e temperatura. Eles são registros somente leitura e são acessados
através da interface serial. No entanto, a função de interrupção pode ser usada para determinar
quando novos dados estão disponíveis.

4 Montagem
Monstou-se o circuito como apresenta a figura 4 numa protoboard, afim de realizar todos
os testes do sensor, o mesmo pode ser visto na figura 5.

Figura 4: Ligação elétrica do módulo ao Arduino.

5
 Figura 5: Montagem do circuito na protoboard.

5 Processamento
Como o processamento de dados foi realizado via software, a seguir será apresentado o
algoritmo utilizado, tanto para a obtenção dos dados quanto para a comunicação serial enviado
ao software Processing. Para tanto deve-se seguir os passos como será instruido a seguir.

5.1 Biblioteca para o software Processing e IDE Arduino

É necessário fazer o upload das bibliotecas I2CDEV e MPU-6050 no Arduino, as quais esta-
rão presentes no anexo 1. Dentro da biblioteca MPU-6050, é preciso comentar o item #define
OUTPUT_READABLE_YAWPITCHROLL, porque ele descomentado irá apresentar no monitor serial
os dados do sensor como é possível visualizar nas figuras 6 e 7. Na visualização dos dados
através do monitor serial, deve se atentar a velocidade de transmissão serial, ou seja, a mesma
velocidade de 115200, selecionando na janela do monitor serial.
Além disso, temos que descomentar o item #define OUTPUT_TEAPOT, para os dados anteri-
ormente visualizados serem transmitidos para a saída serial e processamento do programa.

6
 Também é necessário fazer o upload da biblioteca TOXICLIB para o software Processing,
que está presente no anexo 2 e adicionar o código MPUTeapot.pde. Dentro do MPUTeapot.pde,
há a necessidade de comentar o item String portName = Serial.list()[0]; e descomentar
o item String portName = "COM4";, além de alterar o COM4 para a porta que está conectado
o Arduino.

Figura 6: Teste para o sensor imóvel.

7
 Figura 7: Teste para o sensor inclinado.

6 Testes
Juntamente ao Arduino, o software Processing foi responsável pela modelagem 3D dos dados
adquiridos pelo sensor. A figuras 8 e 9 demonstra o comportamento do objeto gerado em relação
a movimentação do sensor.

8
 Figura 8: Modelagem 3D no software Processing

Figura 9: Objeto 3D correspondente a movimentação do sensor.

9
 Anexo 1
1 // I2C d e v i c e c l a s s ( I2Cdev ) d e m o n s t r a t i o n Arduino s k e t c h f o r MPU6050 c l a s s u s i n g DMP
2
3 ( MotionApps v2 . 0 )
4 // 6/21/2012 by J e f f Rowberg <j e f f @ r o w b e r g . net>
5 // Updates s h o u l d ( h o p e f u l l y ) a l w a y s be a v a i l a b l e a t h t t p s : / / g i t h u b . com/ j r o w b e r g / i 2 c d e v l i b
6 //
7 // Changelog :
8 // 2013−05−08 − added s e a m l e s s F a s t w i r e s u p p o r t
9 // − added n o t e about g y r o c a l i b r a t i o n
10 // 2012−06−21 − added n o t e about Arduino 1 . 0 . 1 + Leonardo c o m p a t i b i l i t y e r r o r
11 // 2012−06−20 − improved FIFO o v e r f l o w h a n d l i n g and s i m p l i f i e d r e a d p r o c e s s
12 // 2012−06−19 − c o m p l e t e l y r e a r r a n g e d DMP i n i t i a l i z a t i o n code and s i m p l i f i c a t i o n
13 // 2012−06−13 − p u l l g y r o and a c c e l data from FIFO p a c k e t i n s t e a d o f r e a d i n g d i r e c t l y
14 // 2012−06−09 − f i x broken FIFO r e a d s e q u e n c e and change i n t e r r u p t d e t e c t i o n t o RISING
15 // 2012−06−05 − add g r a v i t y −compensated i n i t i a l r e f e r e n c e frame a c c e l e r a t i o n o ut put
16 // − add 3D math h e l p e r f i l e t o DMP6 example s k e t c h
17 // − add E u l e r output and Yaw/ P i t c h / R o l l o utput f o r m a t s
18 // 2012−06−04 − remove a c c e l o f f s e t c l e a r i n g f o r b e t t e r r e s u l t s ( t h a n k s Sungon Lee )
19 // 2012−06−01 − f i x e d g y r o s e n s i t i v i t y t o be 2000 deg / s e c i n s t e a d o f 250
20 // 2012−05−30 − b a s i c DMP i n i t i a l i z a t i o n working
21
22 /∗ ============================================
23 I2Cdev d e v i c e l i b r a r y code i s p l a c e d under t h e MIT l i c e n s e
24 C o p y r i g h t ( c ) 2012 J e f f Rowberg
25
26 P e r m i s s i o n i s h e r e b y g r a n t e d , f r e e o f c h a r g e , t o any p e r s o n o b t a i n i n g a copy
27 o f t h i s s o f t w a r e and a s s o c i a t e d documentation f i l e s ( t h e " S o f t w a r e " ) , to deal
28 in the Software without r e s t r i c t i o n , i n c l u d i n g without l i m i t a t i o n the rights
29 t o use , copy , modify , merge , p u b l i s h , d i s t r i b u t e , s u b l i c e n s e , and / o r sell
30 c o p i e s o f t h e S o f t w a r e , and t o p e r m i t p e r s o n s t o whom t h e S o f t w a r e i s
31 f u r n i s h e d t o do so , s u b j e c t t o t h e f o l l o w i n g c o n d i t i o n s :
32
33 The above c o p y r i g h t n o t i c e and t h i s p e r m i s s i o n n o t i c e s h a l l be i n c l u d e d i n
34 a l l c o p i e s or s u b s t a n t i a l p o r t i o n s of the Software .
35
36 THE SOFTWARE I S PROVIDED "AS I S " , WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
37 IMPLIED , INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
38 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
39 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
40 LIABILITY , WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
41 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
42 THE SOFTWARE.
43 ===============================================
44 ∗/
45
46 // I2Cdev and MPU6050 must be i n s t a l l e d a s l i b r a r i e s , o r e l s e t h e . cpp / . h f i l e s
47 // f o r both c l a s s e s must be i n t h e i n c l u d e path o f your p r o j e c t
48 #i n c l u d e " I2Cdev . h"
49
50 #i n c l u d e "MPU6050_6Axis_MotionApps20 . h"
51 //# i n c l u d e "MPU6050 . h" // not n e c e s s a r y i f u s i n g MotionApps i n c l u d e file
52
53 // Arduino Wire l i b r a r y i s r e q u i r e d i f I2Cdev I2CDEV_ARDUINO_WIRE i m p l e m e n t a t i o n
54 // i s used i n I2Cdev . h
55 #i f I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
56 #i n c l u d e " Wire . h"
57 #e n d i f
58
59 // c l a s s d e f a u l t I2C a d d r e s s i s 0 x68
60 // s p e c i f i c I2C a d d r e s s e s may be p a s s e d a s a p a r a m e t e r h e r e
61 // AD0 low = 0 x68 ( d e f a u l t f o r SparkFun b r e a k o u t and I n v e n S e n s e e v a l u a t i o n board )
62 // AD0 h i g h = 0 x69
63 MPU6050 mpu ;
64 //MPU6050 mpu( 0 x69 ) ; // <−− u s e f o r AD0 h i g h
65
66 /∗ =========================================================================
67 NOTE: I n a d d i t i o n t o c o n n e c t i o n 3 . 3 v , GND, SDA, and SCL , t h i s s k e t c h
68 depends on t h e MPU−6050 ’ s INT p i n b e i n g c o n n e c t e d t o t h e Arduino ’ s
69 e x t e r n a l i n t e r r u p t #0 p i n . On t h e Arduino Uno and Mega 2 5 6 0 , t h i s i s
70 d i g i t a l I /O p i n 2 .
71 ∗ ========================================================================= ∗/

10
72
73 /∗ =========================================================================
74 NOTE: Arduino v1 . 0 . 1 with t h e Leonardo board g e n e r a t e s a c o m p i l e e r r o r
75 when u s i n g S e r i a l . w r i t e ( buf , l e n ) . The Teapot o utput u s e s t h i s method .
76 The s o l u t i o n r e q u i r e s a m o d i f i c a t i o n t o t h e Arduino USBAPI . h f i l e , which
77 i s f o r t u n a t e l y s i m p l e , but annoying . This w i l l be f i x e d i n t h e n e x t IDE
78 r e l e a s e . For more i n f o , s e e t h e s e l i n k s :
79
80 h t t p : / / a r d u i n o . c c / forum / i n d e x . php/ t o p i c , 1 0 9 9 8 7 . 0 . html
81 h t t p : / / code . g o o g l e . com/p/ a r d u i n o / i s s u e s / d e t a i l ? i d =958
82 ∗ ========================================================================= ∗/
83
84
85
86 // uncomment "OUTPUT_READABLE_QUATERNION" i f you want t o s e e t h e a c t u a l
87 // q u a t e r n i o n components i n a [ w, x , y , z ] f o r m a t ( not b e s t f o r p a r s i n g
88 // on a remote h o s t such a s P r o c e s s i n g o r s o m e t hi n g though )
89 //# d e f i n e OUTPUT_READABLE_QUATERNION
90
91 // uncomment "OUTPUT_READABLE_EULER" i f you want t o s e e E u l e r a n g l e s
92 // ( i n d e g r e e s ) c a l c u l a t e d from t h e q u a t e r n i o n s coming from t h e FIFO .
93 // Note t h a t E u l e r a n g l e s s u f f e r from g imbal l o c k ( f o r more i n f o , s e e
94 // h t t p : / / en . w i k i p e d i a . o r g / w i k i / Gimbal_lock )
95 //# d e f i n e OUTPUT_READABLE_EULER
96
97 // uncomment "OUTPUT_READABLE_YAWPITCHROLL" i f you want t o s e e t h e yaw/
98 // p i t c h / r o l l a n g l e s ( i n d e g r e e s ) c a l c u l a t e d from t h e q u a t e r n i o n s coming
99 // from t h e FIFO . Note t h i s a l s o r e q u i r e s g r a v i t y v e c t o r c a l c u l a t i o n s .
100 // A l s o n o t e t h a t yaw/ p i t c h / r o l l a n g l e s s u f f e r from g imba l l o c k ( f o r
101 // more i n f o , s e e : h t t p : / / en . w i k i p e d i a . o r g / w i k i / Gimbal_lock )
102 #d e f i n e OUTPUT_READABLE_YAWPITCHROLL
103
104 // uncomment "OUTPUT_READABLE_REALACCEL" i f you want t o s e e a c c e l e r a t i o n
105 // components with g r a v i t y removed . This a c c e l e r a t i o n r e f e r e n c e frame i s
106 // not compensated f o r o r i e n t a t i o n , s o +X i s a l w a y s +X a c c o r d i n g t o t h e
107 // s e n s o r , j u s t w i t h o u t t h e e f f e c t s o f g r a v i t y . I f you want a c c e l e r a t i o n
108 // compensated f o r o r i e n t a t i o n , us OUTPUT_READABLE_WORLDACCEL i n s t e a d .
109 //# d e f i n e OUTPUT_READABLE_REALACCEL
110
111 // uncomment "OUTPUT_READABLE_WORLDACCEL" i f you want t o s e e a c c e l e r a t i o n
112 // components with g r a v i t y removed and a d j u s t e d f o r t h e worl d frame o f
113 // r e f e r e n c e ( yaw i s r e l a t i v e t o i n i t i a l o r i e n t a t i o n , s i n c e no magnetometer
114 // i s p r e s e n t i n t h i s c a s e ) . Could be q u i t e handy i n some c a s e s .
115 //# d e f i n e OUTPUT_READABLE_WORLDACCEL
116
117 // uncomment "OUTPUT_TEAPOT" i f you want o utput t h a t matches t h e
118 // f o r m a t used f o r t h e I n v e n S e n s e t e a p o t demo
119 //# d e f i n e OUTPUT_TEAPOT
120
121
122
123 #d e f i n e INTERRUPT_PIN 2 // u s e p i n 2 on Arduino Uno & most b o a r d s
124 #d e f i n e LED_PIN 13 // ( Arduino i s 1 3 , Teensy i s 1 1 , Teensy++ i s 6 )
125 bool blinkState = f a l s e ;
126
127 // MPU c o n t r o l / s t a t u s v a r s
128 b o o l dmpReady = f a l s e ; // s e t t r u e i f DMP i n i t was s u c c e s s f u l
129 ui nt 8 _ t mpuIntStatus ; // h o l d s a c t u a l i n t e r r u p t s t a t u s b y t e from MPU
130 ui nt 8 _ t d e v S t a t u s ; // r e t u r n s t a t u s a f t e r each d e v i c e o p e r a t i o n ( 0 = s u c c e s s , ! 0 = e r r o r )
131 uint16_t p a c k e t S i z e ; // e x p e c t e d DMP p a c k e t s i z e ( d e f a u l t i s 42 b y t e s )
132 uint16_t f i f o C o u n t ; // count o f a l l b y t e s c u r r e n t l y i n FIFO
133 ui nt 8 _ t f i f o B u f f e r [ 6 4 ] ; // FIFO s t o r a g e b u f f e r
134
135 // o r i e n t a t i o n / motion v a r s
136 Quaternion q ; // [ w, x , y , z ] quaternion container
137 V e c t o r I n t 1 6 aa ; // [ x , y , z ] a c c e l s e n s o r measurements
138 V e c t o r I n t 1 6 aaReal ; // [ x , y , z ] g r a v i t y −f r e e a c c e l s e n s o r measurements
139 V e c t o r I n t 1 6 aaWorld ; // [ x , y , z ] world−frame a c c e l s e n s o r measurements
140 VectorFloat gravity ; // [ x , y , z ] gravity vector
141 float euler [ 3 ] ; // [ p s i , t h e t a , p h i ] Euler angle container
142 f l o a t ypr [ 3 ] ; // [ yaw , p i t c h , r o l l ] yaw/ p i t c h / r o l l c o n t a i n e r and g r a v i t y v e c t o r
143
144 // p a c k e t s t r u c t u r e f o r I n v e n S e n s e t e a p o t demo

11
145 ui nt 8 _ t t e a p o t P a c k e t [ 1 4 ] = { ’ $ ’ , 0 x02 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 x00 , 0 x00 , ’\ r ’ , ’\n ’ } ;
146
147
148
149 // ================================================================
150 // === INTERRUPT DETECTION ROUTINE ===
151 // ================================================================
152
153 v o l a t i l e b o o l mp uI n t e rr upt = f a l s e ; // i n d i c a t e s whether MPU i n t e r r u p t p i n has gone h i g h
154 v o i d dmpDataReady ( ) {
155 mp uI n t e rr upt = t r u e ;
156 }
157
158
159
160 // ================================================================
161 // === INITIAL SETUP ===
162 // ================================================================
163
164 void setup ( ) {
165 // j o i n I2C bus ( I2Cdev l i b r a r y doesn ’ t do t h i s a u t o m a t i c a l l y )
166 #i f I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
167 Wire . b e g i n ( ) ;
168 Wire . s e t C l o c k ( 4 0 0 0 0 0 ) ; // 400 kHz I2C c l o c k . Comment t h i s l i n e i f having compilation d i f f i c u l t i e s
169 # e l i f I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
170 Fastwire : : setup (400 , true ) ;
171 #e n d i f
172
173 // i n i t i a l i z e s e r i a l communication
174 // ( 1 1 5 2 0 0 c h o s e n b e c a u s e i t i s r e q u i r e d f o r Teapot Demo output , but i t ’ s
175 // r e a l l y up t o you d e p e n d i n g on your p r o j e c t )
176 S e r i a l . begin (115200);
177 w h i l e ( ! S e r i a l ) ; // w a i t f o r Leonardo enumeration , o t h e r s c o n t i n u e i m m e d i a t e l y
178
179 // NOTE: 8MHz o r s l o w e r h o s t p r o c e s s o r s , l i k e t h e Teensy @ 3 . 3 v o r Ardunio
180 // Pro Mini r u n n i n g a t 3 . 3 v , c anno t h a n d l e t h i s baud r a t e r e l i a b l y due t o
181 // t h e baud t i m i n g b e i n g t o o m i s a l i g n e d with p r o c e s s o r t i c k s . You must u s e
182 // 38400 o r s l o w e r i n t h e s e c a s e s , o r u s e some k i n d o f e x t e r n a l s e p a r a t e
183 // c r y s t a l s o l u t i o n f o r t h e UART t i m e r .
184
185 // i n i t i a l i z e d e v i c e
186 S e r i a l . p r i n t l n (F( " I n i t i a l i z i n g I2C d e v i c e s . . . " ) ) ;
187 mpu . i n i t i a l i z e ( ) ;
188 pinMode (INTERRUPT_PIN, INPUT ) ;
189
190 // v e r i f y c o n n e c t i o n
191 S e r i a l . p r i n t l n (F( " T e s t i n g d e v i c e c o n n e c t i o n s . . . " ) ) ;
192 S e r i a l . p r i n t l n (mpu . t e s t C o n n e c t i o n ( ) ? F( "MPU6050 c o n n e c t i o n s u c c e s s f u l " ) : F( "MPU6050 c o n n e c t i o n f a i l e d
193
194 // w a i t f o r r e a d y
195 S e r i a l . p r i n t l n (F( " \ nSend any c h a r a c t e r t o b e g i n DMP programming and demo : " ) ) ;
196 w h i l e ( S e r i a l . a v a i l a b l e ( ) && S e r i a l . r e a d ( ) ) ; // empty b u f f e r
197 while ( ! S e r i a l . available ( ) ) ; // w a i t f o r data
198 w h i l e ( S e r i a l . a v a i l a b l e ( ) && S e r i a l . r e a d ( ) ) ; // empty b u f f e r a g a i n
199
200 // l o a d and c o n f i g u r e t h e DMP
201 S e r i a l . p r i n t l n (F( " I n i t i a l i z i n g DMP. . . " ) ) ;
202 d e v S t a t u s = mpu . d m p I n i t i a l i z e ( ) ;
203
204 // s u p p l y your own g y r o o f f s e t s he re , s c a l e d f o r min s e n s i t i v i t y
205 mpu . s e t X G y r o O f f s e t ( 2 2 0 ) ;
206 mpu . s e t Y G y r o O f f s e t ( 7 6 ) ;
207 mpu . s e t Z G y r o O f f s e t ( − 8 5 ) ;
208 mpu . s e t Z A c c e l O f f s e t ( 1 7 8 8 ) ; // 1688 f a c t o r y d e f a u l t f o r my t e s t c h i p
209
210 // make s u r e i t worked ( r e t u r n s 0 i f s o )
211 i f ( d e v S t a t u s == 0 ) {
212 // t u r n on t h e DMP, now t h a t i t ’ s r e a d y
213 S e r i a l . p r i n t l n (F( " E n a b l i n g DMP. . . " ) ) ;
214 mpu . setDMPEnabled ( t r u e ) ;
215
216 // e n a b l e Arduino i n t e r r u p t d e t e c t i o n
217 S e r i a l . p r i n t l n (F( " E n a b l i n g i n t e r r u p t d e t e c t i o n ( Arduino e x t e r n a l i n t e r r u p t 0)..."));

12
218 a t t a c h I n t e r r u p t ( d i g i t a l P i n T o I n t e r r u p t (INTERRUPT_PIN) , dmpDataReady , RISING ) ;
219 mpuIntStatus = mpu . g e t I n t S t a t u s ( ) ;
220
221 // s e t our DMP Ready f l a g s o t h e main l o o p ( ) f u n c t i o n knows i t ’ s okay t o u s e i t
222 S e r i a l . p r i n t l n (F( "DMP r e a d y ! Waiting f o r f i r s t i n t e r r u p t . . . " ) ) ;
223 dmpReady = t r u e ;
224
225 // g e t e x p e c t e d DMP p a c k e t s i z e f o r l a t e r c o m p a r i s o n
226 p a c k e t S i z e = mpu . dmpGetFIFOPacketSize ( ) ;
227 } else {
228 // ERROR!
229 // 1 = i n i t i a l memory l o a d f a i l e d
230 // 2 = DMP c o n f i g u r a t i o n u p d a t e s f a i l e d
231 // ( i f i t ’ s g o i n g t o break , u s u a l l y t h e code w i l l be 1 )
232 S e r i a l . p r i n t (F( "DMP I n i t i a l i z a t i o n f a i l e d ( code " ) ) ;
233 S e r i a l . print ( devStatus ) ;
234 S e r i a l . p r i n t l n (F ( " ) " ) ) ;
235 }
236
237 // c o n f i g u r e LED f o r o ut put
238 pinMode (LED_PIN, OUTPUT) ;
239 }
240
241
242
243 // ================================================================
244 // === MAIN PROGRAM LOOP ===
245 // ================================================================
246
247 void loop ( ) {
248 // i f programming f a i l e d , don ’ t t r y t o do a n y t h i n g
249 i f ( ! dmpReady ) r e t u r n ;
250
251 // w a i t f o r MPU i n t e r r u p t o r e x t r a p a c k e t ( s ) a v a i l a b l e
252 w h i l e ( ! m puI nt e r rup t && f i f o C o u n t < p a c k e t S i z e ) {
253 // o t h e r program b e h a v i o r s t u f f h e r e
254 // .
255 // .
256 // .
257 // i f you a r e r e a l l y p a r a n o i d you can f r e q u e n t l y t e s t i n between o t h e r
258 // s t u f f t o s e e i f m puI nte rrup t i s t r u e , and i f so , " b r e a k ; " from t h e
259 // w h i l e ( ) l o o p t o i m m e d i a t e l y p r o c e s s t h e MPU data
260 // .
261 // .
262 // .
263 }
264
265 // r e s e t i n t e r r u p t f l a g and g e t INT_STATUS b y t e
266 mp uI n t e rr upt = f a l s e ;
267 mpuIntStatus = mpu . g e t I n t S t a t u s ( ) ;
268
269 // g e t c u r r e n t FIFO count
270 f i f o C o u n t = mpu . getFIFOCount ( ) ;
271
272 // c h e c k f o r o v e r f l o w ( t h i s s h o u l d n e v e r happen u n l e s s our code i s t o o i n e f f i c i e n t )
273 i f ( ( mpuIntStatus & 0 x10 ) | | f i f o C o u n t == 1 0 2 4 ) {
274 // r e s e t s o we can c o n t i n u e c l e a n l y
275 mpu . resetFIFO ( ) ;
276 S e r i a l . p r i n t l n (F( " FIFO o v e r f l o w ! " ) ) ;
277
278 // o t h e r w i s e , c h e c k f o r DMP data r e a d y i n t e r r u p t ( t h i s s h o u l d happen f r e q u e n t l y )
279 } e l s e i f ( mpuIntStatus & 0 x02 ) {
280 // w a i t f o r c o r r e c t a v a i l a b l e data l e n g t h , s h o u l d be a VERY s h o r t w a i t
281 w h i l e ( f i f o C o u n t < p a c k e t S i z e ) f i f o C o u n t = mpu . getFIFOCount ( ) ;
282
283 // r e a d a p a c k e t from FIFO
284 mpu . getFIFOBytes ( f i f o B u f f e r , p a c k e t S i z e ) ;
285
286 // t r a c k FIFO count h e r e i n c a s e t h e r e i s > 1 p a c k e t a v a i l a b l e
287 // ( t h i s l e t s us i m m e d i a t e l y r e a d more w i t h o u t w a i t i n g f o r an i n t e r r u p t )
288 f i f o C o u n t −= p a c k e t S i z e ;
289
290 #i f d e f OUTPUT_READABLE_QUATERNION

13
291 // d i s p l a y q u a t e r n i o n v a l u e s i n e a s y m a t r i x form : w x y z
292 mpu . dmpGetQuaternion(&q , f i f o B u f f e r ) ;
293 S e r i a l . p r i n t ( " quat \ t " ) ;
294 S e r i a l . p r i n t ( q .w) ;
295 S e r i a l . p r i n t ("\ t " ) ;
296 Serial . print (q . x );
297 S e r i a l . p r i n t ("\ t " ) ;
298 Serial . print (q . y );
299 S e r i a l . p r i n t ("\ t " ) ;
300 Serial . println (q . z );
301 #e n d i f
302
303 #i f d e f OUTPUT_READABLE_EULER
304 // d i s p l a y E u l e r a n g l e s i n d e g r e e s
305 mpu . dmpGetQuaternion(&q , f i f o B u f f e r ) ;
306 mpu . dmpGetEuler ( e u l e r , &q ) ;
307 S e r i a l . p r i n t (" e u l e r \ t " ) ;
308 S e r i a l . p r i n t ( e u l e r [ 0 ] ∗ 180/M_PI ) ;
309 S e r i a l . p r i n t ("\ t " ) ;
310 S e r i a l . p r i n t ( e u l e r [ 1 ] ∗ 180/M_PI ) ;
311 S e r i a l . p r i n t ("\ t " ) ;
312 S e r i a l . p r i n t l n ( e u l e r [ 2 ] ∗ 180/M_PI ) ;
313 #e n d i f
314
315 #i f d e f OUTPUT_READABLE_YAWPITCHROLL
316 // d i s p l a y E u l e r a n g l e s i n d e g r e e s
317 mpu . dmpGetQuaternion(&q , f i f o B u f f e r ) ;
318 mpu . dmpGetGravity(& g r a v i t y , &q ) ;
319 mpu . dmpGetYawPitchRoll ( ypr , &q , &g r a v i t y ) ;
320 S e r i a l . p r i n t ( " ypr \ t " ) ;
321 S e r i a l . p r i n t ( ypr [ 0 ] ∗ 180/M_PI ) ;
322 S e r i a l . p r i n t ("\ t " ) ;
323 S e r i a l . p r i n t ( ypr [ 1 ] ∗ 180/M_PI ) ;
324 S e r i a l . p r i n t ("\ t " ) ;
325 S e r i a l . p r i n t l n ( ypr [ 2 ] ∗ 180/M_PI ) ;
326 #e n d i f
327
328 #i f d e f OUTPUT_READABLE_REALACCEL
329 // d i s p l a y r e a l a c c e l e r a t i o n , a d j u s t e d t o remove g r a v i t y
330 mpu . dmpGetQuaternion(&q , f i f o B u f f e r ) ;
331 mpu . dmpGetAccel(&aa , f i f o B u f f e r ) ;
332 mpu . dmpGetGravity(& g r a v i t y , &q ) ;
333 mpu . dmpGetLinearAccel(&aaReal , &aa , &g r a v i t y ) ;
334 S e r i a l . p r i n t (" a r e a l \ t " ) ;
335 S e r i a l . p r i n t ( aaReal . x ) ;
336 S e r i a l . p r i n t ("\ t " ) ;
337 S e r i a l . p r i n t ( aaReal . y ) ;
338 S e r i a l . p r i n t ("\ t " ) ;
339 S e r i a l . p r i n t l n ( aaReal . z ) ;
340 #e n d i f
341
342 #i f d e f OUTPUT_READABLE_WORLDACCEL
343 // d i s p l a y i n i t i a l world−frame a c c e l e r a t i o n , a d j u s t e d t o remove g r a v i t y
344 // and r o t a t e d based on known o r i e n t a t i o n from q u a t e r n i o n
345 mpu . dmpGetQuaternion(&q , f i f o B u f f e r ) ;
346 mpu . dmpGetAccel(&aa , f i f o B u f f e r ) ;
347 mpu . dmpGetGravity(& g r a v i t y , &q ) ;
348 mpu . dmpGetLinearAccel(&aaReal , &aa , &g r a v i t y ) ;
349 mpu . dmpGetLinearAccelInWorld(&aaWorld , &aaReal , &q ) ;
350 S e r i a l . p r i n t ( " awo rld \ t " ) ;
351 S e r i a l . p r i n t ( aaWorld . x ) ;
352 S e r i a l . p r i n t ("\ t " ) ;
353 S e r i a l . p r i n t ( aaWorld . y ) ;
354 S e r i a l . p r i n t ("\ t " ) ;
355 S e r i a l . p r i n t l n ( aaWorld . z ) ;
356 #e n d i f
357
358 #i f d e f OUTPUT_TEAPOT
359 // d i s p l a y q u a t e r n i o n v a l u e s i n I n v e n S e n s e Teapot demo f o r m a t :
360 teapotPacket [ 2 ] = f i f o B u f f e r [0];
361 teapotPacket [ 3 ] = f i f o B u f f e r [1];
362 teapotPacket [ 4 ] = f i f o B u f f e r [4];
363 teapotPacket [ 5 ] = f i f o B u f f e r [5];

14
364 teapotPacket [ 6 ] = f i f o B u f f e r [ 8 ] ;
365 teapotPacket [ 7 ] = f i f o B u f f e r [ 9 ] ;
366 teapotPacket [ 8 ] = f i f o B u f f e r [ 1 2 ] ;
367 teapotPacket [ 9 ] = f i f o B u f f e r [ 1 3 ] ;
368 S e r i a l . write ( teapotPacket , 1 4 ) ;
369 t e a p o t P a c k e t [ 1 1 ] + + ; // packetCount , l o o p s a t 0xFF on p u r p o s e
370 #e n d i f
371
372 // b l i n k LED t o i n d i c a t e a c t i v i t y
373 blinkState = ! blinkState ;
374 d i g i t a l W r i t e (LED_PIN, b l i n k S t a t e ) ;
375 }
376 }

15
 Anexo 2
1 // I2C d e v i c e c l a s s ( I2Cdev ) d e m o n s t r a t i o n P r o c e s s i n g s k e t c h f o r MPU6050 DMP o ut put
2 // 6/20/2012 by J e f f Rowberg <j e f f @ r o w b e r g . net>
3 // Updates s h o u l d ( h o p e f u l l y ) a l w a y s be a v a i l a b l e a t h t t p s : / / g i t h u b . com/ j r o w b e r g / i 2 c d e v l i b
4 //
5 // Changelog :
6 // 2012−06−20 − i n i t i a l r e l e a s e
7
8 /∗ ============================================
9 I2Cdev d e v i c e l i b r a r y code i s p l a c e d under t h e MIT l i c e n s e
10 C o p y r i g h t ( c ) 2012 J e f f Rowberg
11
12 P e r m i s s i o n i s h e r e b y g r a n t e d , f r e e o f c h a r g e , t o any p e r s o n o b t a i n i n g a copy
13 o f t h i s s o f t w a r e and a s s o c i a t e d documentation f i l e s ( t h e " S o f t w a r e " ) , to deal
14 in the Software without r e s t r i c t i o n , i n c l u d i n g without l i m i t a t i o n the rights
15 t o use , copy , modify , merge , p u b l i s h , d i s t r i b u t e , s u b l i c e n s e , and / o r sell
16 c o p i e s o f t h e S o f t w a r e , and t o p e r m i t p e r s o n s t o whom t h e S o f t w a r e i s
17 f u r n i s h e d t o do so , s u b j e c t t o t h e f o l l o w i n g c o n d i t i o n s :
18
19 The above c o p y r i g h t n o t i c e and t h i s p e r m i s s i o n n o t i c e s h a l l be i n c l u d e d i n
20 a l l c o p i e s or s u b s t a n t i a l p o r t i o n s of the Software .
21
22 THE SOFTWARE I S PROVIDED "AS I S " , WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
23 IMPLIED , INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
24 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
25 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
26 LIABILITY , WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
27 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
28 THE SOFTWARE.
29 ===============================================
30 ∗/
31
32 import processing . s e r i a l . ∗ ;
33 import processing . opengl . ∗ ;
34 import t o x i . geom . ∗ ;
35 import toxi . processing . ∗ ;
36
37 // NOTE: r e q u i r e s T o x i c L i b s t o be i n s t a l l e d i n o r d e r t o run p r o p e r l y .
38 // 1 . Download from h t t p : / / t o x i c l i b s . o r g / downloads
39 // 2. Extract into [ u s e r d i r ]/ Processing / l i b r a r i e s
40 // ( l o c a t i o n may be d i f f e r e n t on Mac/ Linux )
41 // 3 . Run and bask i n awesomeness
42
43 ToxiclibsSupport gfx ;
44
45 S e r i a l port ; // The s e r i a l p o r t
46 c h a r [ ] t e a p o t P a c k e t = new c h a r [ 1 4 ] ; // I n v e n S e n s e Teapot p a c k e t
47 int serialCount = 0; // c u r r e n t p a c k e t b y t e p o s i t i o n
48 i n t synced = 0 ;
49 int interval = 0;
50
51 f l o a t [ ] q = new f l o a t [ 4 ] ;
52 Q u a t e r n i o n quat = new Q u a t e r n i o n ( 1 , 0 , 0 , 0 ) ;
53
54 f l o a t [ ] g r a v i t y = new f l o a t [ 3 ] ;
55 f l o a t [ ] e u l e r = new f l o a t [ 3 ] ;
56 f l o a t [ ] ypr = new f l o a t [ 3 ] ;
57
58 void setup ( ) {
59 // 300 px s q u a r e v i e w p o r t u s i n g OpenGL r e n d e r i n g
60 s i z e ( 3 0 0 , 3 0 0 , OPENGL) ;
61 g f x = new T o x i c l i b s S u p p o r t ( t h i s ) ;
62
63 // s e t u p l i g h t s and a n t i a l i a s i n g
64 lights ();
65 smooth ( ) ;
66
67 // d i s p l a y s e r i a l p o r t l i s t f o r debugging / c l a r i t y
68 println ( Serial . l i s t ());
69
70 // g e t t h e f i r s t a v a i l a b l e p o r t ( u s e EITHER t h i s OR t h e s p e c i f i c p o r t code below )
71 S t r i n g portName = S e r i a l . l i s t ( ) [ 0 ] ;

16
72
73 // g e t a s p e c i f i c s e r i a l p o r t ( u s e EITHER t h i s OR t h e f i r s t −a v a i l a b l e code above )
74 // S t r i n g portName = "COM4" ;
75
76 // open t h e s e r i a l p o r t
77 p o r t = new S e r i a l ( t h i s , portName , 1 1 5 2 0 0 ) ;
78
79 // send s i n g l e c h a r a c t e r t o t r i g g e r DMP i n i t / s t a r t
80 // ( e x p e c t e d by MPU6050_DMP6 example Arduino s k e t c h )
81 port . write ( ’ r ’ ) ;
82 }
83
84 v o i d draw ( ) {
85 i f ( m i l l i s ( ) − i n t e r v a l > 1000) {
86 // r e s e n d s i n g l e c h a r a c t e r t o t r i g g e r DMP i n i t / s t a r t
87 // i n c a s e t h e MPU i s h a l t e d / r e s e t w h i l e a p p l e t i s r u n n i n g
88 port . write ( ’ r ’ ) ;
89 interval = millis ();
90 }
91
92 // b l a c k background
93 background ( 0 ) ;
94
95 // t r a n s l a t e e v e r y t h i n g t o t h e middle o f t h e v i e w p o r t
96 pushMatrix ( ) ;
97 t r a n s l a t e ( width / 2 , h e i g h t / 2 ) ;
98
99 // 3− s t e p r o t a t i o n from yaw/ p i t c h / r o l l a n g l e s ( gimbal l o c k ! )
100 // . . . and o t h e r w e i r d n e s s I haven ’ t f i g u r e d out y e t
101 // r o t a t e Y (−ypr [ 0 ] ) ;
102 // r o t a t e Z (−ypr [ 1 ] ) ;
103 // r o t a t e X (−ypr [ 2 ] ) ;
104
105 // t o x i c l i b s d i r e c t a n g l e / a x i s r o t a t i o n from q u a t e r n i o n (NO g imba l l o c k ! )
106 // ( a x i s o r d e r [ 1 , 3 , 2 ] and i n v e r s i o n [ −1 , +1 , +1] i s a c o n s e q u e n c e o f
107 // d i f f e r e n t c o o r d i n a t e system o r i e n t a t i o n a s s u m p t i o n s between P r o c e s s i n g
108 // and I n v e n S e n s e DMP)
109 f l o a t [ ] a x i s = quat . t o A x i s A n g l e ( ) ;
110 r o t a t e ( a x i s [ 0 ] , −a x i s [ 1 ] , a x i s [ 3 ] , a x i s [ 2 ] ) ;
111
112 // draw main body i n r e d
113 f i l l (255 , 0 , 0 , 200);
114 box ( 1 0 , 1 0 , 2 0 0 ) ;
115
116 // draw f r o n t −f a c i n g t i p i n b l u e
117 f i l l (0 , 0 , 255 , 2 0 0 ) ;
118 pushMatrix ( ) ;
119 t r a n s l a t e ( 0 , 0 , −120);
120 r o t a t e X ( PI / 2 ) ;
121 drawCylinder (0 , 20 , 20 , 8 ) ;
122 popMatrix ( ) ;
123
124 // draw wings and t a i l f i n i n g r e e n
125 f i l l (0 , 255 , 0 , 2 0 0 ) ;
126 b e g i n S h a p e (TRIANGLES ) ;
127 v e r t e x ( −100 , 2 , 3 0 ) ; v e r t e x ( 0 , 2 , −80); v e r t e x ( 1 0 0 , 2 , 30); // wing top l a y e r
128 v e r t e x ( −100 , −2, 3 0 ) ; v e r t e x ( 0 , −2, −80); v e r t e x ( 1 0 0 , −2, 3 0 ) ; // wing bottom l a y e r
129 v e r t e x ( −2 , 0 , 9 8 ) ; v e r t e x ( −2 , −30 , 9 8 ) ; v e r t e x ( −2 , 0 , 7 0 ) ; // t a i l l e f t l a y e r
130 v e r t e x ( 2 , 0 , 9 8 ) ; v e r t e x ( 2 , −30 , 9 8 ) ; v e r t e x ( 2 , 0 , 7 0 ) ; // t a i l r i g h t l a y e r
131 endShape ( ) ;
132 b e g i n S h a p e (QUADS) ;
133 v e r t e x ( −100 , 2 , 3 0 ) ; v e r t e x ( −100 , −2, 3 0 ) ; v e r t e x ( 0 , −2, −80); v e r t e x ( 0 , 2 , −80);
134 v e r t e x ( 1 0 0 , 2 , 3 0 ) ; v e r t e x ( 1 0 0 , −2, 3 0 ) ; v e r t e x ( 0 , −2, −80); v e r t e x ( 0 , 2 , −80);
135 v e r t e x ( −100 , 2 , 3 0 ) ; v e r t e x ( −100 , −2, 3 0 ) ; v e r t e x ( 1 0 0 , −2, 3 0 ) ; v e r t e x ( 1 0 0 , 2 , 30);
136 v e r t e x ( −2 , 0 , 98); vertex (2 , 0 , 9 8 ) ; v e r t e x ( 2 , −30 , 9 8 ) ; v e r t e x ( −2 , −30 , 9 8 ) ;
137 v e r t e x ( −2 , 0 , 98); vertex (2 , 0 , 98); vertex (2 , 0 , 7 0 ) ; v e r t e x ( −2 , 0 , 70);
138 v e r t e x ( −2 , −30 , 9 8 ) ; v e r t e x ( 2 , −30 , 9 8 ) ; v e r t e x ( 2 , 0 , 7 0 ) ; v e r t e x ( −2 , 0 , 70);
139 endShape ( ) ;
140
141 popMatrix ( ) ;
142 }
143
144 void s e r i a l E v e n t ( S e r i a l port ) {

17
145 interval = millis ();
146 while ( port . a v a i l a b l e ( ) > 0) {
147 i n t ch = p o r t . r e a d ( ) ;
148
149 i f ( s y n c e d == 0 && ch != ’ $ ’ ) r e t u r n ; // i n i t i a l s y n c h r o n i z a t i o n − a l s o used t o r e s y n c / r e a l i g n i
150 synced = 1 ;
151 p r i n t ( ( c h a r ) ch ) ;
152
153 if ( ( s e r i a l C o u n t == 1 && ch != 2 )
154 | | ( s e r i a l C o u n t == 12 && ch != ’ \ r ’ )
155 | | ( s e r i a l C o u n t == 13 && ch != ’ \ n ’ ) ) {
156 serialCount = 0;
157 synced = 0 ;
158 return ;
159 }
160
161 i f ( s e r i a l C o u n t > 0 | | ch == ’ $ ’ ) {
162 t e a p o t P a c k e t [ s e r i a l C o u n t ++] = ( c h a r ) ch ;
163 i f ( s e r i a l C o u n t == 1 4 ) {
164 s e r i a l C o u n t = 0 ; // r e s t a r t p a c k e t b y t e p o s i t i o n
165
166 // g e t q u a t e r n i o n from data packet
167 q [ 0 ] = ( ( t e a p o t P a c k e t [ 2 ] << 8) | teapotPacket [ 3 ] ) / 16384.0 f ;
168 q [ 1 ] = ( ( t e a p o t P a c k e t [ 4 ] << 8) | teapotPacket [ 5 ] ) / 16384.0 f ;
169 q [ 2 ] = ( ( t e a p o t P a c k e t [ 6 ] << 8) | teapotPacket [ 7 ] ) / 16384.0 f ;
170 q [ 3 ] = ( ( t e a p o t P a c k e t [ 8 ] << 8) | teapotPacket [ 9 ] ) / 16384.0 f ;
171 f o r ( i n t i = 0 ; i < 4 ; i ++) i f ( q [ i ] >= 2 ) q [ i ] = −4 + q [ i ] ;
172
173 // s e t our t o x i l i b s q u a t e r n i o n t o new data
174 quat . s e t ( q [ 0 ] , q [ 1 ] , q [ 2 ] , q [ 3 ] ) ;
175
176 /∗
177 // below c a l c u l a t i o n s u n n e c e s s a r y f o r o r i e n t a t i o n o n l y u s i n g t o x i l i b s
178
179 // c a l c u l a t e gravity vector
180 gravity [ 0 ] = 2 ∗ (q [1]∗ q [ 3 ] − q [0]∗ q [ 2 ] ) ;
181 gravity [ 1 ] = 2 ∗ (q [0]∗ q [ 1 ] + q [2]∗ q [ 3 ] ) ;
182 gravity [ 2 ] = q[0]∗q [0] − q[1]∗q [1] − q[2]∗q [2] + q[3]∗q [3];
183
184 // c a l c u l a t e E u l e r a n g l e s
185 e u l e r [ 0 ] = atan2 ( 2 ∗ q [ 1 ] ∗ q [ 2 ] − 2∗ q [ 0 ] ∗ q [ 3 ] , 2∗ q [ 0 ] ∗ q [ 0 ] + 2∗ q [ 1 ] ∗ q [ 1 ] − 1 ) ;
186 e u l e r [ 1 ] = −a s i n ( 2 ∗ q [ 1 ] ∗ q [ 3 ] + 2∗ q [ 0 ] ∗ q [ 2 ] ) ;
187 e u l e r [ 2 ] = atan2 ( 2 ∗ q [ 2 ] ∗ q [ 3 ] − 2∗ q [ 0 ] ∗ q [ 1 ] , 2∗ q [ 0 ] ∗ q [ 0 ] + 2∗ q [ 3 ] ∗ q [ 3 ] − 1 ) ;
188
189 // c a l c u l a t e yaw/ p i t c h / r o l l a n g l e s
190 ypr [ 0 ] = atan2 ( 2 ∗ q [ 1 ] ∗ q [ 2 ] − 2∗ q [ 0 ] ∗ q [ 3 ] , 2∗ q [ 0 ] ∗ q [ 0 ] + 2∗ q [ 1 ] ∗ q [ 1 ] − 1 ) ;
191 ypr [ 1 ] = atan ( g r a v i t y [ 0 ] / s q r t ( g r a v i t y [ 1 ] ∗ g r a v i t y [ 1 ] + g r a v i t y [ 2 ] ∗ g r a v i t y [ 2 ] ) ) ;
192 ypr [ 2 ] = atan ( g r a v i t y [ 1 ] / s q r t ( g r a v i t y [ 0 ] ∗ g r a v i t y [ 0 ] + g r a v i t y [ 2 ] ∗ g r a v i t y [ 2 ] ) ) ;
193
194 // output v a r i o u s components f o r d e b u g g i n g
195 // p r i n t l n ( " q : \ t " + round ( q [ 0 ] ∗ 1 0 0 . 0 f ) / 1 0 0 . 0 f + "\ t " + round ( q [ 1 ] ∗ 1 0 0 . 0 f ) / 1 0 0 . 0 f + "\ t " + ro
196 // p r i n t l n ( " e u l e r : \ t " + e u l e r [ 0 ] ∗ 1 8 0 . 0 f / PI + "\ t " + e u l e r [ 1 ] ∗ 1 8 0 . 0 f / PI + "\ t " + e u l e r [ 2 ] ∗ 1 8
197 // p r i n t l n ( " ypr : \ t " + ypr [ 0 ] ∗ 1 8 0 . 0 f / PI + "\ t " + ypr [ 1 ] ∗ 1 8 0 . 0 f / PI + "\ t " + ypr [ 2 ] ∗ 1 8 0 . 0 f / PI )
198 ∗/
199 }
200 }
201 }
202 }
203
204 v o i d d r a w C y l i n d e r ( f l o a t topRadius , f l o a t bottomRadius , f l o a t t a l l , i n t s i d e s ) {
205 f l o a t angle = 0;
206 f l o a t a n g l e I n c r e m e n t = TWO_PI / s i d e s ;
207 b e g i n S h a p e (QUAD_STRIP ) ;
208 f o r ( i n t i = 0 ; i < s i d e s + 1 ; ++i ) {
209 v e r t e x ( t opRa dius ∗ c o s ( a n g l e ) , 0 , topRa dius ∗ s i n ( a n g l e ) ) ;
210 v e r t e x ( bottomRadius ∗ c o s ( a n g l e ) , t a l l , bottomRadius ∗ s i n ( a n g l e ) ) ;
211 a n g l e += a n g l e I n c r e m e n t ;
212 }
213 endShape ( ) ;
214
215 // I f i t i s not a cone , draw t h e c i r c u l a r top cap
216 i f ( to pR adius != 0 ) {
217 angle = 0;

18
218 b e g i n S h a p e (TRIANGLE_FAN) ;
219
220 // C enter p o i n t
221 vertex (0 , 0 , 0);
222 f o r ( i n t i = 0 ; i < s i d e s + 1 ; i ++) {
223 v e r t e x ( t opRa dius ∗ c o s ( a n g l e ) , 0 , topRa dius ∗ s i n ( a n g l e ) ) ;
224 a n g l e += a n g l e I n c r e m e n t ;
225 }
226 endShape ( ) ;
227 }
228
229 // I f i t i s not a cone , draw t h e c i r c u l a r bottom cap
230 i f ( bottomRadius != 0 ) {
231 angle = 0;
232 b e g i n S h a p e (TRIANGLE_FAN) ;
233
234 // C enter p o i n t
235 vertex (0 , tall , 0);
236 f o r ( i n t i = 0 ; i < s i d e s + 1 ; i ++) {
237 v e r t e x ( bottomRadius ∗ c o s ( a n g l e ) , t a l l , bottomRadius ∗ s i n ( a n g l e ) ) ;
238 a n g l e += a n g l e I n c r e m e n t ;
239 }
240 endShape ( ) ;
241 }
242 }

19
Referências
[1] BOYLESTAD, R. L. NASHELSKY, L. Dispositivos Eletrônicos e Teoria de
Circuitos. 11a Edição. Pearson.

[2] Datasheet MPU-6050. https://store.invensense.com/datasheets/invensense/MPU-

6050_DataSheet_V3%204.pdf. Acessado em 06 de agosto de 2019.

20