/
OrientationVisualiser.pde
170 lines (140 loc) · 4.07 KB
/
OrientationVisualiser.pde
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
import processing.serial.*;
Serial myPort;
float yaw = 0.0;
float pitch = 0.0;
float roll = 0.0;
void setup()
{
size(600, 500, P3D);
// if you have only ONE serial port active
//myPort = new Serial(this, Serial.list()[0], 9600); // if you have only ONE serial port active
// if you know the serial port name
//myPort = new Serial(this, "COM5:", 9600); // Windows "COM#:"
//myPort = new Serial(this, "\\\\.\\COM41", 9600); // Windows, COM10 or higher
myPort = new Serial(this, "/dev/ttyACM0", 9600); // Linux "/dev/ttyACM#"
//myPort = new Serial(this, "/dev/cu.usbmodem1217321", 9600); // Mac "/dev/cu.usbmodem######"
textSize(16); // set text size
textMode(SHAPE); // set text mode to shape
}
void draw()
{
serialEvent(); // read and parse incoming serial message
background(255); // set background to white
lights();
translate(width/2, height/2); // set position to centre
pushMatrix(); // begin object
float c1 = cos(radians(roll));
float s1 = sin(radians(roll));
float c2 = cos(radians(-pitch));
float s2 = sin(radians(-pitch));
float c3 = cos(radians(yaw));
float s3 = sin(radians(yaw));
applyMatrix( c2*c3, s1*s3+c1*c3*s2, c3*s1*s2-c1*s3, 0,
-s2, c1*c2, c2*s1, 0,
c2*s3, c1*s2*s3-c3*s1, c1*c3+s1*s2*s3, 0,
0, 0, 0, 1);
drawPropShield();
//drawArduino();
popMatrix(); // end of object
// Print values to console
print(roll);
print("\t");
print(-pitch);
print("\t");
print(yaw);
println();
}
void serialEvent()
{
int newLine = 13; // new line character in ASCII
String message;
do {
message = myPort.readStringUntil(newLine); // read from port until new line
if (message != null) {
String[] list = split(trim(message), " ");
if (list.length >= 4 && list[0].equals("Orientation:")) {
yaw = float(list[1]); // convert to float yaw
pitch = float(list[2]); // convert to float pitch
roll = float(list[3]); // convert to float roll
}
}
} while (message != null);
}
void drawArduino()
{
/* function contains shape(s) that are rotated with the IMU */
stroke(0, 90, 90); // set outline colour to darker teal
fill(0, 130, 130); // set fill colour to lighter teal
box(300, 10, 200); // draw Arduino board base shape
stroke(0); // set outline colour to black
fill(80); // set fill colour to dark grey
translate(60, -10, 90); // set position to edge of Arduino box
box(170, 20, 10); // draw pin header as box
translate(-20, 0, -180); // set position to other edge of Arduino box
box(210, 20, 10); // draw other pin header as box
}
void drawPropShield()
{
// 3D art by Benjamin Rheinland
stroke(0); // black outline
fill(0, 128, 0); // fill color PCB green
box(190, 6, 70); // PCB base shape
fill(255, 215, 0); // gold color
noStroke();
//draw 14 contacts on Y- side
translate(65, 0, 30);
for (int i=0; i<14; i++) {
sphere(4.5); // draw gold contacts
translate(-10, 0, 0); // set new position
}
//draw 14 contacts on Y+ side
translate(10, 0, -60);
for (int i=0; i<14; i++) {
sphere(4.5); // draw gold contacts
translate(10, 0, 0); // set position
}
//draw 5 contacts on X+ side (DAC, 3v3, gnd)
translate(-10,0,10);
for (int i=0; i<5; i++) {
sphere(4.5);
translate(0,0,10);
}
//draw 4 contacts on X+ side (G C D 5)
translate(25,0,-15);
for (int i=0; i<4; i++) {
sphere(4.5);
translate(0,0,-10);
}
//draw 4 contacts on X- side (5V - + GND)
translate(-180,0,10);
for (int i=0; i<4; i++) {
sphere(4.5);
translate(0,0,10);
}
//draw audio amp IC
stroke(128);
fill(24); //Epoxy color
translate(30,-6,-25);
box(13,6,13);
//draw pressure sensor IC
stroke(64);
translate(32,0,0);
fill(192);
box(10,6,18);
//draw gyroscope IC
stroke(128);
translate(27,0,0);
fill(24);
box(16,6,16);
//draw flash memory IC
translate(40,0,-15);
box(20,6,20);
//draw accelerometer/magnetometer IC
translate(-5,0,25);
box(12,6,12);
//draw 5V level shifter ICs
translate(42.5,2,0);
box(6,4,8);
translate(0,0,-20);
box(6,4,8);
}