3D Artificial Horizon - Gyroscope with MPU6050 GY-521

1// it all started here....
2//https://forum.processing.org/two/discussion/13353/creating-arc-sections-in-3d
3
4import
5  org.firmata.*;
6import cc.arduino.*;
7import processing.serial.*;
8import
9  java.awt.event.KeyEvent;
10import java.io.IOException;
11import processing.opengl.*;
12
13
14Serial
15  myPort;
16
17String data="";
18
19float Pitch;     // replaces roll and pitch
20  
21float Bank; 
22float Azimuth; 
23
24float Phi;    //Dimensional axis
25float
26  Theta;
27float Psi;
28
29float radius = 250.0;      
30float rho = radius;
31float
32  factor = TWO_PI / 40.0;    // detail is 40
33float x, y, z;
34 
35PVector[] sphereVertexPoints;
36
37////////////////////////////////////////////////////////////////////////
38
39void
40  setup() {
41  
42   size(400, 400, P3D);
43   smooth();
44    
45myPort = new
46  Serial(this, "COM4", 9600); // starts the serial communication myPort.bufferUntil('\
47');
48
49
50        }
51     
52 ////////////////////////////////////////////////////////////////////
53
54  
55void draw() {
56  
57background(0); 
58  
59  stroke(100);
60  strokeWeight(4);
61  
62  fill(200);
63  rect(130, 10, 150, 50, 7);  // (x,y,w,h, round corner dia)
64
65  fill(CLOSE); 
66  
67  fill(0); 
68  rect(150, 30, 70, 25, 7); 
69  fill(CLOSE);
70  
71  
72  strokeWeight(CLOSE); // very useful 
73  fill(CLOSE); 
74  stroke(CLOSE);
75  
76  
77  fill(0);
78  textSize(14);
79  text("Altitude: ", 150, 50, 50);
80  
81  fill(CLOSE);
82  float y1 = y++;
83  float Alt = abs(Pitch/25+y1*10);
84
85  
86  fill(0,255,255); 
87  text(Alt, 160,70,50);  // alt, x,y,z
88  fill(CLOSE);
89  
90  
91  
92  fill(0, 0, 255); 
93  textSize(12); 
94  text(" m/s", 220,
95  70, 50 );
96  fill(CLOSE); 
97  
98   
99  translate(width/2, height/2, -250);
100  // camera placement
101  lights();
102  
103  textLayer(); 
104  MakeAnglesDependentOnMPU6050();
105  
106  
107  
108   fill(100);
109   circle(0,0,700);  // dark gray circle 
110   fill(CLOSE);
111      
112   
113   fill(150);
114   circle(0,0,670);   // light grey circle 
115
116   fill(CLOSE); 
117  
118  beginShape(); 
119
120  fill(255,255,0); 
121    vertex(-14,
122  -10, 300);
123    vertex(14, -10, 300);       // yellow triangle, each vertex is
124  x and y coords
125    vertex(0, -20, 300);
126    vertex(0, -20, 300);
127   stroke(0);
128  
129   strokeWeight(2); 
130   stroke(CLOSE);
131   
132   
133  endShape(CLOSE);
134  
135  
136   beginShape(); 
137    fill(255,255,0); 
138    stroke(0); 
139    strokeWeight(2);
140  
141    stroke(CLOSE); 
142    vertex(-50, -5, 300);
143    vertex( 50, -5, 300);
144       // yellow  line
145    vertex( 50, -10, 300);
146    vertex(-50, -10, 300);
147
148    fill(CLOSE); 
149  endShape(CLOSE); 
150  
151 
152  fill(210);
153  arc(-120,
154  0, 380, 525, PI/2, 3*PI/2);   // left gauge fill
155  arc(120, 0, -380, -525, PI/2,
156  3*PI/2);  // right gauge fill
157  fill(CLOSE); 
158  
159 //////////////////////////////////////////////////////////////
160
161
162  pushMatrix();  
163
164   
165   
166     rotateX(radians(Bank)); 
167     rotateZ(radians(Pitch));
168    // comment out to change to mouse
169     rotateY(radians(Azimuth)); 
170 
171
172  
173
174 // rotateX(radians(mouseY));  // comment in, to change to mouse. 
175
176  // rotateY(radians(mouseX));
177 
178  for(float PHI = 0.0; PHI < HALF_PI; PHI +=
179  factor) {
180    
181    beginShape(QUAD_STRIP);
182    stroke(240); 
183    strokeWeight(1);
184
185   
186    for(float THETA = 0.0; THETA < TWO_PI + factor; THETA += factor) {
187
188     
189      x = rho * sin(PHI) * cos(THETA);
190      z = rho * sin(PHI) * sin(THETA);
191
192      y = -rho * cos(PHI);
193 
194      vertex(x, y, z);
195 
196      x = rho
197  * sin(PHI + factor) * cos(THETA);
198      z = rho * sin(PHI + factor) * sin(THETA);
199
200      y = -rho * cos(PHI + factor);
201 
202      vertex(x, y, z);
203      fill(100,
204  100,255); 
205    
206   }
207 
208    endShape(CLOSE);
209  }
210  
211  
212
213
214
215for(float
216  phi = 0.0; phi < HALF_PI; phi += factor) {
217    
218    beginShape(QUAD_STRIP);
219
220    
221    for(float theta = 0.0; theta < TWO_PI + factor; theta += factor) {
222
223      
224      x = rho * sin(phi) * cos(theta);
225      z = rho * sin(phi) * sin(theta);
226
227      y = -rho * cos(phi);
228 
229      vertex(-x, -y, -z);
230 
231      x = rho
232  * sin(phi + factor) * cos(theta);
233      z = rho * sin(phi + factor) * sin(theta);
234
235      y = -rho * cos(phi + factor);
236 
237      vertex(-x, -y, -z);
238      fill(255,128,0);
239
240      
241      
242  }
243    
244    
245    endShape(CLOSE);
246 
247}
248
249
250   popMatrix(); 
251            
252 /////////////////////////////////////////////////////////////////////
253          
254
255    } // void draw
256  
257  
258
259//////////////////////////////////////////////////////////////////////////
260
261
262    void textLayer() { 
263    
264     
265         MakeAnglesDependentOnMPU6050();
266
267       
268     /*pushMatrix();
269           
270         rotateX(Bank/10); 
271
272         rotateY(-Pitch/10); 
273         rotateZ(Azimuth/10); 
274       
275         fill(255);
276
277    
278              textSize(15);           
279              text( "50", 0,
280  110, 230);          
281              text( "90", 0, 180, 200);
282   
283           popMatrix();
284  */
285           
286      /////////////////////////////////////////////////////////
287        
288           
289           pushMatrix(); 
290             
291             float
292  y1 = y++;   
293             float alt = abs(Pitch/25+y1*10+90);
294           
295
296             
297             beginShape(); 
298             rotateZ(alt); 
299
300             stroke(255,255,0);              // throttle measurement  yellow
301
302             strokeWeight(8); 
303             line(0,0,-310, 0);
304             stroke(CLOSE);
305  
306             endShape();
307             strokeWeight(CLOSE); 
308            
309
310            popMatrix(); 
311               
312 /////////////////////////////////////////////////////////////////////////////
313  
314
315    
316    
317    int radius = 300; 
318    int lines = 5*17; 
319    
320
321  for (int a=120; a<240; a=a+360/lines) { 
322     
323    float x = radius * cos(radians(a));
324        // gauge lines yeah baby 
325    float y = radius * sin(radians(a)); 
326
327    
328   
329 
330      stroke(50);
331      line(0,0,x,y); 
332      line(0,0,-x,-y);
333
334      strokeWeight(3); 
335      stroke(CLOSE); 
336     
337     }
338    
339///////////////////////////////////////////////////////////
340    
341    
342     
343     for (int a=120; a<160; a=a+360/lines) { 
344       
345
346       float x = radius * cos(radians(a));        // red gauge lines yeah baby 
347
348       float y = radius * sin(radians(a)); 
349       
350       stroke(255,0,0);
351
352       line(0,0,-x,-y); 
353       strokeWeight(3); 
354       stroke(CLOSE); 
355
356     }
357       
358  for (int a=160; a<190; a=a+360/lines) { 
359       
360       float
361  x = radius * cos(radians(a));        // orange gauge lines yeah baby 
362       float
363  y = radius * sin(radians(a)); 
364       
365       stroke(255,128,0);
366       line(0,0,-x,-y);
367  
368         strokeWeight(3); 
369       stroke(CLOSE); 
370   
371   
372   }
373
374       
375       
376      for (int a=220; a<240; a=a+360/lines) { 
377       
378
379       float x = radius * cos(radians(a));        // red gauge max throttle 
380
381       float y = radius * sin(radians(a)); 
382       
383       stroke(255,0,0);
384
385       strokeWeight(3); 
386       line(0,0,x,y); 
387       stroke(CLOSE); 
388
389   
390   
391   }
392       
393       
394////////////////////////////////////////////////////////////////
395
396    
397 
398           
399         }
400
401
402///////////////////////////////////////////////////////////////////////
403
404
405       void serialEvent(Serial myport)       {     //Reading the datas by Processing.
406  
407  
408          String input = myport.readStringUntil('\
409');
410          
411
412            if   (input != null)      {
413             
414                input
415  = trim(input);
416               String[] values = split(input, " ");
417             
418
419             if  (values.length == 3)  {
420                  
421                 
422
423                  float phi = float(values[0]);
424                  float theta
425  = float(values[1]); 
426                  float psi = float(values[2]); 
427                  
428
429                  print(phi);    
430                  print(theta);
431                  println(psi);
432
433                 
434                  Phi = phi;
435                  Theta = theta;
436
437                  Psi = psi;
438                    
439                         
440
441                         
442                         
443                          }
444
445                      }
446                   }
447                   
448///////////////////////////////////////////////////////////////////////
449
450
451
452      void MakeAnglesDependentOnMPU6050()   { 
453         
454         Bank = round(-Theta);
455
456         Pitch =round(-Phi-3);
457         Azimuth = round(Psi/700);
458         
459
460         
461     //   Bank =  -Phi/10; 
462       // Pitch =  Theta/10;       //
463  use these values for the ADXL345 
464       // Azimuth = Psi/10;
465        
466
467        }   
468        
469 /////////////////////////////////////////////////////////////////////////

1// NavBall Code 
2
3//https://www.instructables.com/How-to-Measure-Angle-With-MPU-6050GY-521/
4
5//
6  library link for MPU6050 or GY-521
7// https://github.com/jarzebski/Arduino-MPU6050
8
9#include<Wire.h>
10  // wire library
11
12const int MPU_addr=0x68;   // MPU address
13
14int16_t
15  AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;  // 16 bit data array
16
17int minVal=265; 
18int
19  maxVal=402;
20
21double x; double y; double z;
22
23void setup() { 
24  
25
26  Serial.begin(9600);
27  
28  Wire.begin();
29  Wire.beginTransmission(MPU_addr);
30
31  Wire.write(0x6B); 
32  Wire.write(0); 
33  Wire.endTransmission(true); 
34
35  
36  
37   } 
38
39void loop() {
40 
41  Wire.beginTransmission(MPU_addr);
42
43  Wire.write(0x3B); 
44  Wire.endTransmission(false); 
45  Wire.requestFrom(MPU_addr,14,true);
46  
47 
48  AcX=Wire.read()<<8|Wire.read(); 
49  AcY=Wire.read()<<8|Wire.read();
50  
51  AcZ=Wire.read()<<8|Wire.read(); 
52  
53  int xAng = map(AcX,minVal,maxVal,-90,90);
54
55  int yAng = map(AcY,minVal,maxVal,-90,90); 
56  int zAng = map(AcZ,minVal,maxVal,-90,90);
57
58x=
59  RAD_TO_DEG * (atan2(-yAng, -zAng)+PI); 
60y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
61  
62z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
63
64      
65      Serial.print(x);
66  
67      Serial.print(" "); 
68     
69      Serial.print(y); 
70      Serial.print("
71  "); 
72      
73      Serial.print(z); 
74      Serial.print(" ");
75      
76
77      Serial.print("\
78"); 
79
80   }

1// it all started here....
2//https://forum.processing.org/two/discussion/13353/creating-arc-sections-in-3d
3
4import  org.firmata.*;
5import cc.arduino.*;
6import processing.serial.*;
7import  java.awt.event.KeyEvent;
8import java.io.IOException;
9import processing.opengl.*;
10
11
12Serial  myPort;
13
14String data="";
15
16float Pitch;     // replaces roll and pitch  
17float Bank; 
18float Azimuth; 
19
20float Phi;    //Dimensional axis
21float  Theta;
22float Psi;
23
24float radius = 250.0;      
25float rho = radius;
26float  factor = TWO_PI / 40.0;    // detail is 40
27float x, y, z;
28 
29PVector[] sphereVertexPoints;
30
31////////////////////////////////////////////////////////////////////////
32
33void  setup() {
34  
35   size(400, 400, P3D);
36   smooth();
37    
38myPort = new  Serial(this, "COM4", 9600); // starts the serial communication myPort.bufferUntil('\n');
39
40        }
41     
42 ////////////////////////////////////////////////////////////////////
43  
44void draw() {
45  
46background(0); 
47  
48  stroke(100);
49  strokeWeight(4);  
50  fill(200);
51  rect(130, 10, 150, 50, 7);  // (x,y,w,h, round corner dia)
52  fill(CLOSE); 
53  
54  fill(0); 
55  rect(150, 30, 70, 25, 7); 
56  fill(CLOSE);  
57  
58  strokeWeight(CLOSE); // very useful 
59  fill(CLOSE); 
60  stroke(CLOSE);  
61  
62  fill(0);
63  textSize(14);
64  text("Altitude: ", 150, 50, 50);  
65  fill(CLOSE);
66  float y1 = y++;
67  float Alt = abs(Pitch/25+y1*10);
68  
69  fill(0,255,255); 
70  text(Alt, 160,70,50);  // alt, x,y,z
71  fill(CLOSE);  
72  
73  
74  fill(0, 0, 255); 
75  textSize(12); 
76  text(" m/s", 220,  70, 50 );
77  fill(CLOSE); 
78  
79   
80  translate(width/2, height/2, -250);  // camera placement
81  lights();
82  
83  textLayer(); 
84  MakeAnglesDependentOnMPU6050();  
85  
86  
87   fill(100);
88   circle(0,0,700);  // dark gray circle 
89   fill(CLOSE);      
90   
91   fill(150);
92   circle(0,0,670);   // light grey circle 
93   fill(CLOSE); 
94  
95  beginShape(); 
96
97  fill(255,255,0); 
98    vertex(-14,  -10, 300);
99    vertex(14, -10, 300);       // yellow triangle, each vertex is  x and y coords
100    vertex(0, -20, 300);
101    vertex(0, -20, 300);
102   stroke(0);  
103   strokeWeight(2); 
104   stroke(CLOSE);
105   
106   
107  endShape(CLOSE);  
108  
109   beginShape(); 
110    fill(255,255,0); 
111    stroke(0); 
112    strokeWeight(2);  
113    stroke(CLOSE); 
114    vertex(-50, -5, 300);
115    vertex( 50, -5, 300);       // yellow  line
116    vertex( 50, -10, 300);
117    vertex(-50, -10, 300);
118    fill(CLOSE); 
119  endShape(CLOSE); 
120  
121 
122  fill(210);
123  arc(-120,  0, 380, 525, PI/2, 3*PI/2);   // left gauge fill
124  arc(120, 0, -380, -525, PI/2,  3*PI/2);  // right gauge fill
125  fill(CLOSE); 
126  
127 //////////////////////////////////////////////////////////////
128
129  pushMatrix();  
130
131   
132   
133     rotateX(radians(Bank)); 
134     rotateZ(radians(Pitch));    // comment out to change to mouse
135     rotateY(radians(Azimuth)); 
136 
137  
138
139 // rotateX(radians(mouseY));  // comment in, to change to mouse. 
140  // rotateY(radians(mouseX));
141 
142  for(float PHI = 0.0; PHI < HALF_PI; PHI +=  factor) {
143    
144    beginShape(QUAD_STRIP);
145    stroke(240); 
146    strokeWeight(1);
147   
148    for(float THETA = 0.0; THETA < TWO_PI + factor; THETA += factor) {
149     
150      x = rho * sin(PHI) * cos(THETA);
151      z = rho * sin(PHI) * sin(THETA);
152      y = -rho * cos(PHI);
153 
154      vertex(x, y, z);
155 
156      x = rho  * sin(PHI + factor) * cos(THETA);
157      z = rho * sin(PHI + factor) * sin(THETA);
158      y = -rho * cos(PHI + factor);
159 
160      vertex(x, y, z);
161      fill(100,  100,255); 
162    
163   }
164 
165    endShape(CLOSE);
166  }
167  
168  
169
170
171
172for(float  phi = 0.0; phi < HALF_PI; phi += factor) {
173    
174    beginShape(QUAD_STRIP);
175    
176    for(float theta = 0.0; theta < TWO_PI + factor; theta += factor) {
177      
178      x = rho * sin(phi) * cos(theta);
179      z = rho * sin(phi) * sin(theta);
180      y = -rho * cos(phi);
181 
182      vertex(-x, -y, -z);
183 
184      x = rho  * sin(phi + factor) * cos(theta);
185      z = rho * sin(phi + factor) * sin(theta);
186      y = -rho * cos(phi + factor);
187 
188      vertex(-x, -y, -z);
189      fill(255,128,0);
190      
191      
192  }
193    
194    
195    endShape(CLOSE);
196 
197}
198
199   popMatrix(); 
200            
201 /////////////////////////////////////////////////////////////////////          
202
203    } // void draw
204  
205  
206
207//////////////////////////////////////////////////////////////////////////
208
209    void textLayer() { 
210    
211     
212         MakeAnglesDependentOnMPU6050();
213       
214     /*pushMatrix();
215           
216         rotateX(Bank/10); 
217         rotateY(-Pitch/10); 
218         rotateZ(Azimuth/10); 
219       
220         fill(255);
221    
222              textSize(15);           
223              text( "50", 0,  110, 230);          
224              text( "90", 0, 180, 200);
225   
226           popMatrix();  */
227           
228      /////////////////////////////////////////////////////////        
229           
230           pushMatrix(); 
231             
232             float  y1 = y++;   
233             float alt = abs(Pitch/25+y1*10+90);
234           
235             
236             beginShape(); 
237             rotateZ(alt); 
238             stroke(255,255,0);              // throttle measurement  yellow
239             strokeWeight(8); 
240             line(0,0,-310, 0);
241             stroke(CLOSE);  
242             endShape();
243             strokeWeight(CLOSE); 
244            
245            popMatrix(); 
246               
247 /////////////////////////////////////////////////////////////////////////////  
248
249    
250    
251    int radius = 300; 
252    int lines = 5*17; 
253    
254  for (int a=120; a<240; a=a+360/lines) { 
255     
256    float x = radius * cos(radians(a));        // gauge lines yeah baby 
257    float y = radius * sin(radians(a)); 
258    
259   
260 
261      stroke(50);
262      line(0,0,x,y); 
263      line(0,0,-x,-y);
264      strokeWeight(3); 
265      stroke(CLOSE); 
266     
267     }
268    
269///////////////////////////////////////////////////////////    
270    
271     
272     for (int a=120; a<160; a=a+360/lines) { 
273       
274       float x = radius * cos(radians(a));        // red gauge lines yeah baby 
275       float y = radius * sin(radians(a)); 
276       
277       stroke(255,0,0);
278       line(0,0,-x,-y); 
279       strokeWeight(3); 
280       stroke(CLOSE); 
281     }
282       
283  for (int a=160; a<190; a=a+360/lines) { 
284       
285       float  x = radius * cos(radians(a));        // orange gauge lines yeah baby 
286       float  y = radius * sin(radians(a)); 
287       
288       stroke(255,128,0);
289       line(0,0,-x,-y);  
290         strokeWeight(3); 
291       stroke(CLOSE); 
292   
293   
294   }
295       
296       
297      for (int a=220; a<240; a=a+360/lines) { 
298       
299       float x = radius * cos(radians(a));        // red gauge max throttle 
300       float y = radius * sin(radians(a)); 
301       
302       stroke(255,0,0);
303       strokeWeight(3); 
304       line(0,0,x,y); 
305       stroke(CLOSE); 
306   
307   
308   }
309       
310       
311////////////////////////////////////////////////////////////////
312    
313 
314           
315         }
316
317
318///////////////////////////////////////////////////////////////////////
319
320       void serialEvent(Serial myport)       {     //Reading the datas by Processing.  
321  
322          String input = myport.readStringUntil('\n');
323          
324            if   (input != null)      {
325             
326                input  = trim(input);
327               String[] values = split(input, " ");
328             
329             if  (values.length == 3)  {
330                  
331                 
332                  float phi = float(values[0]);
333                  float theta  = float(values[1]); 
334                  float psi = float(values[2]); 
335                  
336                  print(phi);    
337                  print(theta);
338                  println(psi);
339                 
340                  Phi = phi;
341                  Theta = theta;
342                  Psi = psi;
343                    
344                         
345                         
346                         
347                          }
348                      }
349                   }
350                   
351///////////////////////////////////////////////////////////////////////
352
353
354      void MakeAnglesDependentOnMPU6050()   { 
355         
356         Bank = round(-Theta);
357         Pitch =round(-Phi-3);
358         Azimuth = round(Psi/700);
359         
360         
361     //   Bank =  -Phi/10; 
362       // Pitch =  Theta/10;       //  use these values for the ADXL345 
363       // Azimuth = Psi/10;
364        
365        }   
366        
367 /////////////////////////////////////////////////////////////////////////

1// NavBall Code 
2
3//https://www.instructables.com/How-to-Measure-Angle-With-MPU-6050GY-521/
4
5//  library link for MPU6050 or GY-521
6// https://github.com/jarzebski/Arduino-MPU6050
7
8#include<Wire.h>  // wire library
9
10const int MPU_addr=0x68;   // MPU address
11
12int16_t  AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;  // 16 bit data array
13
14int minVal=265; 
15int  maxVal=402;
16
17double x; double y; double z;
18
19void setup() { 
20  
21  Serial.begin(9600);
22  
23  Wire.begin();
24  Wire.beginTransmission(MPU_addr);
25  Wire.write(0x6B); 
26  Wire.write(0); 
27  Wire.endTransmission(true); 
28  
29  
30   } 
31
32void loop() {
33 
34  Wire.beginTransmission(MPU_addr);
35  Wire.write(0x3B); 
36  Wire.endTransmission(false); 
37  Wire.requestFrom(MPU_addr,14,true);  
38 
39  AcX=Wire.read()<<8|Wire.read(); 
40  AcY=Wire.read()<<8|Wire.read();  
41  AcZ=Wire.read()<<8|Wire.read(); 
42  
43  int xAng = map(AcX,minVal,maxVal,-90,90);
44  int yAng = map(AcY,minVal,maxVal,-90,90); 
45  int zAng = map(AcZ,minVal,maxVal,-90,90);
46
47x=  RAD_TO_DEG * (atan2(-yAng, -zAng)+PI); 
48y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);  
49z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
50
51      
52      Serial.print(x);  
53      Serial.print(" "); 
54     
55      Serial.print(y); 
56      Serial.print("  "); 
57      
58      Serial.print(z); 
59      Serial.print(" ");
60      
61      Serial.print("\
62"); 
63
64   }