Arduino 101 CurieIMU Orientation Visualiser

Copy
1Madgwick filter;

Copy
1CurieIMU.begin();
2
3CurieIMU.setGyroRate(25);
4
5CurieIMU.setAccelerometerRate(25);
6
7filter.begin(25);

Copy
1CurieIMU.setAccelerometerRange(2);
2
3CurieIMU.setGyroRange(250);

Copy
1microsPerReading = 1000000 / 25;

Copy
1CurieIMU.readMotionSensor(aix, aiy, aiz, gix, giy, giz);
2
3// convert from raw data to gravity and degrees/second units
4
5ax = convertRawAcceleration(aix);
6
7ay = convertRawAcceleration(aiy);
8
9az = convertRawAcceleration(aiz);
10
11gx = convertRawGyro(gix);
12
13gy = convertRawGyro(giy);
14
15gz = convertRawGyro(giz);

Copy
1filter.updateIMU(gx, gy, gz, ax, ay, az);

Copy
1roll = filter.getRoll();
2
3pitch = filter.getPitch();
4
5heading = filter.getYaw();

Copy
1myPort = new Serial(this, Serial.list()[0], 9600);

Copy
1myPort = new Serial(this, Serial.list()[0], 9600);

Copy
1myPort = new Serial(this, "COM5:", 9600);                    // Windows
2
3myPort = new Serial(this, "/dev/ttyACM0", 9600);             // Linux
4
5myPort = new Serial(this, "/dev/cu.usbmodem1217321", 9600);  // Mac

Copy
1void serialEvent()
2{
3
4  int newLine = 13; // new line character in ASCII
5
6  String message;
7
8  do {
9
10    message = myPort.readStringUntil(newLine); // read from port until new line
11
12    if (message != null) {
13
14      String[] list = split(trim(message), " ");
15
16      if (list.length >= 4 && list[0].equals("Orientation:")) {
17
18        yaw = float(list[1]); // convert to float yaw
19
20        pitch = float(list[2]); // convert to float pitch
21
22        roll = float(list[3]); // convert to float roll
23
24      }
25
26    }
27
28  } while (message != null);
29}

Copy
1#include <CurieIMU.h>
2#include <MadgwickAHRS.h>
3
4Madgwick filter;
5unsigned long microsPerReading, microsPrevious;
6float accelScale, gyroScale;
7
8void setup() {
9
10  Serial.begin(9600);
11
12  // start the IMU and filter
13
14  CurieIMU.begin();
15
16  CurieIMU.setGyroRate(25);
17
18  CurieIMU.setAccelerometerRate(25);
19
20  filter.begin(25);
21
22  // Set the accelerometer range to 2G
23
24  CurieIMU.setAccelerometerRange(2);
25
26  // Set the gyroscope range to 250 degrees/second
27
28  CurieIMU.setGyroRange(250);
29
30  // initialize variables to pace updates to correct rate
31
32  microsPerReading = 1000000 / 25;
33
34  microsPrevious = micros();
35}
36
37void loop() {
38
39  int aix, aiy, aiz;
40
41  int gix, giy, giz;
42
43  float ax, ay, az;
44
45  float gx, gy, gz;
46
47  float roll, pitch, heading;
48
49  unsigned long microsNow;
50
51  // check if it's time to read data and update the filter
52
53  microsNow = micros();
54
55  if (microsNow - microsPrevious >= microsPerReading) {
56
57    // read raw data from CurieIMU
58
59    CurieIMU.readMotionSensor(aix, aiy, aiz, gix, giy, giz);
60
61    // convert from raw data to gravity and degrees/second units
62
63    ax = convertRawAcceleration(aix);
64
65    ay = convertRawAcceleration(aiy);
66
67    az = convertRawAcceleration(aiz);
68
69    gx = convertRawGyro(gix);
70
71    gy = convertRawGyro(giy);
72
73    gz = convertRawGyro(giz);
74
75    // update the filter, which computes orientation
76
77    filter.updateIMU(gx, gy, gz, ax, ay, az);
78
79    // print the heading, pitch and roll
80
81    roll = filter.getRoll();
82
83    pitch = filter.getPitch();
84
85    heading = filter.getYaw();
86
87    Serial.print("Orientation: ");
88
89    Serial.print(heading);
90
91    Serial.print(" ");
92
93    Serial.print(pitch);
94
95    Serial.print(" ");
96
97    Serial.println(roll);
98
99    // increment previous time, so we keep proper pace
100
101    microsPrevious = microsPrevious + microsPerReading;
102
103  }
104}
105
106float convertRawAcceleration(int aRaw) {
107
108  // since we are using 2G range
109
110  // -2g maps to a raw value of -32768
111
112  // +2g maps to a raw value of 32767
113
114
115
116  float a = (aRaw * 2.0) / 32768.0;
117
118  return a;
119}
120
121float convertRawGyro(int gRaw) {
122
123  // since we are using 250 degrees/seconds range
124
125  // -250 maps to a raw value of -32768
126
127  // +250 maps to a raw value of 32767
128
129
130
131  float g = (gRaw * 250.0) / 32768.0;
132
133  return g;
134}

Copy
1import processing.serial.*;
2Serial myPort;
3
4float yaw = 0.0;
5float pitch = 0.0;
6float roll = 0.0;
7
8void setup()
9{
10
11  size(600, 500, P3D);
12
13  // if you have only ONE serial port active
14
15  myPort = new Serial(this, Serial.list()[0], 9600); // if you have only ONE serial port active
16
17  // if you know the serial port name
18
19  //myPort = new Serial(this, "COM5:", 9600);                    // Windows
20
21  //myPort = new Serial(this, "/dev/ttyACM0", 9600);             // Linux
22
23  //myPort = new Serial(this, "/dev/cu.usbmodem1217321", 9600);  // Mac
24
25  textSize(16); // set text size
26
27  textMode(SHAPE); // set text mode to shape
28}
29
30void draw()
31{
32
33  serialEvent();  // read and parse incoming serial message
34
35  background(255); // set background to white
36
37  lights();
38
39  translate(width/2, height/2); // set position to centre
40
41  pushMatrix(); // begin object
42
43  float c1 = cos(radians(roll));
44
45  float s1 = sin(radians(roll));
46
47  float c2 = cos(radians(pitch));
48
49  float s2 = sin(radians(pitch));
50
51  float c3 = cos(radians(yaw));
52
53  float s3 = sin(radians(yaw));
54
55  applyMatrix( c2*c3, s1*s3+c1*c3*s2, c3*s1*s2-c1*s3, 0,
56
57               -s2, c1*c2, c2*s1, 0,
58
59               c2*s3, c1*s2*s3-c3*s1, c1*c3+s1*s2*s3, 0,
60
61               0, 0, 0, 1);
62
63  drawArduino();
64
65  popMatrix(); // end of object
66
67  // Print values to console
68
69  print(roll);
70
71  print("\t");
72
73  print(pitch);
74
75  print("\t");
76
77  print(yaw);
78
79  println();
80}
81
82void serialEvent()
83{
84
85  int newLine = 13; // new line character in ASCII
86
87  String message;
88
89  do {
90
91    message = myPort.readStringUntil(newLine); // read from port until new line
92
93    if (message != null) {
94
95      String[] list = split(trim(message), " ");
96
97      if (list.length >= 4 && list[0].equals("Orientation:")) {
98
99        yaw = float(list[1]); // convert to float yaw
100
101        pitch = float(list[2]); // convert to float pitch
102
103        roll = float(list[3]); // convert to float roll
104
105      }
106
107    }
108
109  } while (message != null);
110}
111
112void drawArduino()
113{
114
115  /* function contains shape(s) that are rotated with the IMU */
116
117  stroke(0, 90, 90); // set outline colour to darker teal
118
119  fill(0, 130, 130); // set fill colour to lighter teal
120
121  box(300, 10, 200); // draw Arduino board base shape
122
123  stroke(0); // set outline colour to black
124
125  fill(80); // set fill colour to dark grey
126
127  translate(60, -10, 90); // set position to edge of Arduino box
128
129  box(170, 20, 10); // draw pin header as box
130
131  translate(-20, 0, -180); // set position to other edge of Arduino box
132
133  box(210, 20, 10); // draw other pin header as box
134}