Statistics
| Branch: | Revision:

adafruit_bno055 / examples / bunny / processing / cuberotate / cuberotate.pde @ 56d5ab50

History | View | Annotate | Download (5.109 KB)

1
import processing.serial.*;
2
import java.awt.datatransfer.*;
3
import java.awt.Toolkit;
4
import processing.opengl.*;
5
import saito.objloader.*;
6
import g4p_controls.*;
7

    
8
float roll  = 0.0F;
9
float pitch = 0.0F;
10
float yaw   = 0.0F;
11
float temp  = 0.0F;
12
float alt   = 0.0F;
13

    
14
OBJModel model;
15

    
16
// Serial port state.
17
Serial       port;
18
final String serialConfigFile = "serialconfig.txt";
19
boolean      printSerial = false;
20

    
21
// UI controls.
22
GPanel    configPanel;
23
GDropList serialList;
24
GLabel    serialLabel;
25
GLabel    calLabel;
26
GCheckbox printSerialCheckbox;
27

    
28
void setup()
29
{
30
  size(640, 480, OPENGL);
31
  frameRate(30);
32
  model = new OBJModel(this);
33
  model.load("bunny.obj");
34
  model.scale(20);
35
  
36
  // Serial port setup.
37
  // Grab list of serial ports and choose one that was persisted earlier or default to the first port.
38
  int selectedPort = 0;
39
  String[] availablePorts = Serial.list();
40
  if (availablePorts == null) {
41
    println("ERROR: No serial ports available!");
42
    exit();
43
  }
44
  String[] serialConfig = loadStrings(serialConfigFile);
45
  if (serialConfig != null && serialConfig.length > 0) {
46
    String savedPort = serialConfig[0];
47
    // Check if saved port is in available ports.
48
    for (int i = 0; i < availablePorts.length; ++i) {
49
      if (availablePorts[i].equals(savedPort)) {
50
        selectedPort = i;
51
      } 
52
    }
53
  }
54
  // Build serial config UI.
55
  configPanel = new GPanel(this, 10, 10, width-20, 90, "Configuration (click to hide/show)");
56
  serialLabel = new GLabel(this,  0, 20, 80, 25, "Serial port:");
57
  configPanel.addControl(serialLabel);
58
  serialList = new GDropList(this, 90, 20, 200, 200, 6);
59
  serialList.setItems(availablePorts, selectedPort);
60
  configPanel.addControl(serialList);
61
  calLabel = new GLabel(this, 300, 20, 350, 25, "Calibration: Sys=? Gyro=? Accel=? Mag=?");
62
  configPanel.addControl(calLabel); 
63
  printSerialCheckbox = new GCheckbox(this, 5, 50, 200, 20, "Print serial data");
64
  printSerialCheckbox.setSelected(printSerial);
65
  configPanel.addControl(printSerialCheckbox);
66
  // Set serial port.
67
  setSerialPort(serialList.getSelectedText());
68
}
69
 
70
void draw()
71
{
72
  background(0,0,0);
73

    
74
  // Set a new co-ordinate space
75
  pushMatrix();
76

    
77
  // Simple 3 point lighting for dramatic effect.
78
  // Slightly red light in upper right, slightly blue light in upper left, and white light from behind.
79
  pointLight(255, 200, 200,  400, 400,  500);
80
  pointLight(200, 200, 255, -400, 400,  500);
81
  pointLight(255, 255, 255,    0,   0, -500);
82
  
83
  // Move bunny from 0,0 in upper left corner to roughly center of screen.
84
  translate(300, 380, 0);
85
  
86
  // Rotate shapes around the X/Y/Z axis (values in radians, 0..Pi*2)
87
  //rotateZ(radians(roll));
88
  //rotateX(radians(pitch)); // extrinsic rotation
89
  //rotateY(radians(yaw));
90
  float c1 = cos(radians(roll));
91
  float s1 = sin(radians(roll));
92
  float c2 = cos(radians(pitch)); // intrinsic rotation
93
  float s2 = sin(radians(pitch));
94
  float c3 = cos(radians(yaw));
95
  float s3 = sin(radians(yaw));
96
  applyMatrix( c2*c3, s1*s3+c1*c3*s2, c3*s1*s2-c1*s3, 0,
97
               -s2, c1*c2, c2*s1, 0,
98
               c2*s3, c1*s2*s3-c3*s1, c1*c3+s1*s2*s3, 0,
99
               0, 0, 0, 1);
100

    
101
  pushMatrix();
102
  noStroke();
103
  model.draw();
104
  popMatrix();
105
  popMatrix();
106
  //print("draw");
107
}
108

    
109
void serialEvent(Serial p) 
110
{
111
  String incoming = p.readString();
112
  if (printSerial) {
113
    println(incoming);
114
  }
115
  
116
  if ((incoming.length() > 8))
117
  {
118
    String[] list = split(incoming, " ");
119
    if ( (list.length > 0) && (list[0].equals("Orientation:")) ) 
120
    {
121
      roll  = float(list[3]); // Roll = Z
122
      pitch = float(list[2]); // Pitch = Y 
123
      yaw   = float(list[1]); // Yaw/Heading = X
124
    }
125
    if ( (list.length > 0) && (list[0].equals("Alt:")) ) 
126
    {
127
      alt  = float(list[1]);
128
    }
129
    if ( (list.length > 0) && (list[0].equals("Temp:")) ) 
130
    {
131
      temp  = float(list[1]);
132
    }
133
    if ( (list.length > 0) && (list[0].equals("Calibration:")) )
134
    {
135
      int sysCal   = int(list[1]);
136
      int gyroCal  = int(list[2]);
137
      int accelCal = int(list[3]);
138
      int magCal   = int(trim(list[4]));
139
      calLabel.setText("Calibration: Sys=" + sysCal + " Gyro=" + gyroCal + " Accel=" + accelCal + " Mag=" + magCal);
140
    }
141
  }
142
}
143

    
144
// Set serial port to desired value.
145
void setSerialPort(String portName) {
146
  // Close the port if it's currently open.
147
  if (port != null) {
148
    port.stop();
149
  }
150
  try {
151
    // Open port.
152
    port = new Serial(this, portName, 115200);
153
    port.bufferUntil('\n');
154
    // Persist port in configuration.
155
    saveStrings(serialConfigFile, new String[] { portName });
156
  }
157
  catch (RuntimeException ex) {
158
    // Swallow error if port can't be opened, keep port closed.
159
    port = null; 
160
  }
161
}
162

    
163
// UI event handlers
164

    
165
void handlePanelEvents(GPanel panel, GEvent event) {
166
  // Panel events, do nothing.
167
}
168

    
169
void handleDropListEvents(GDropList list, GEvent event) { 
170
  // Drop list events, check if new serial port is selected.
171
  if (list == serialList) {
172
    setSerialPort(serialList.getSelectedText()); 
173
  }
174
}
175

    
176
void handleToggleControlEvents(GToggleControl checkbox, GEvent event) { 
177
  // Checkbox toggle events, check if print events is toggled.
178
  if (checkbox == printSerialCheckbox) {
179
    printSerial = printSerialCheckbox.isSelected(); 
180
  }
181
}