#include "CurieIMU.h"
#include "math.h"
int16_t ax, ay, az;
int16_t gx, gy, gz;
const int ledPin = 13;
boolean blinkState = false; // state of the LED
void setup() {
Serial.begin(9600); // initialize Serial communication
while (!Serial); // wait for the serial port to open
// initialize device
Serial.println("Initializing IMU device...");
CurieIMU.begin();
// verify connection
Serial.println("Testing device connections...");
if (CurieIMU.testConnection()) {
Serial.println("CurieIMU connection successful");
} else {
Serial.println("CurieIMU connection failed");
}
// use the code below to calibrate accel/gyro offset values
Serial.println("Internal sensor offsets BEFORE calibration...");
Serial.print(CurieIMU.getXAccelOffset());
Serial.print("\t"); // -76
Serial.print(CurieIMU.getYAccelOffset());
Serial.print("\t"); // -235
Serial.print(CurieIMU.getZAccelOffset());
Serial.print("\t"); // 168
Serial.print(CurieIMU.getXGyroOffset());
Serial.print("\t"); // 0
Serial.print(CurieIMU.getYGyroOffset());
Serial.print("\t"); // 0
Serial.println(CurieIMU.getZGyroOffset());
Serial.println("About to calibrate. Make sure your board is stable and upright");
delay(5000);
// The board must be resting in a horizontal position for
// the following calibration procedure to work correctly!
Serial.print("Starting Gyroscope calibration...");
CurieIMU.autoCalibrateGyroOffset();
Serial.println(" Done");
Serial.print("Starting Acceleration calibration...");
CurieIMU.autoCalibrateXAccelOffset(0);
CurieIMU.autoCalibrateYAccelOffset(0);
CurieIMU.autoCalibrateZAccelOffset(1);
Serial.println(" Done");
Serial.println("Internal sensor offsets AFTER calibration...");
Serial.print(CurieIMU.getXAccelOffset());
Serial.print("\t"); // -76
// accelerometer values
// gyrometer values
// activity LED pin
Serial.print(CurieIMU.getYAccelOffset());
Serial.print("\t"); // -2359
Serial.print(CurieIMU.getZAccelOffset());
Serial.print("\t"); // 1688
Serial.print(CurieIMU.getXGyroOffset());
Serial.print("\t"); // 0
Serial.print(CurieIMU.getYGyroOffset());
Serial.print("\t"); // 0
Serial.println(CurieIMU.getZGyroOffset());
Serial.println("Enabling Gyroscope/Acceleration offset compensation");
CurieIMU.setGyroOffsetEnabled(true);
CurieIMU.setAccelOffsetEnabled(true);
// configure Arduino LED for activity indicator
pinMode(ledPin, OUTPUT);
}
void loop() {
// read raw accel/gyro measurements from device
CurieIMU.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
double unitx, unity;
unitx= double(ax)/sqrt(ax*ax+ay*ay+az*az);
unity= double(ay)/sqrt(ax*ax+ay*ay+az*az);
double pitch, yaw,roll;
yaw = 0;
pitch = -asin(unity/sqrt(1-unitx*unitx)) ;
roll = atan(unitx/sqrt(1-unitx*unitx));
Serial.print(float(yaw));
Serial.print(","); // print comma so values can be parsed
Serial.print(float(pitch));
Serial.print(","); // print comma so values can be parsed
Serial.println(float(roll));
delay (10);
}
import processing.serial.*;
Serial myPort;
int newLine = 13; // new line character in ASCII
float yaw;
float pitch;
float roll;
String message;
String [] ypr = new String [3];
void setup()
{
size(600, 500, P3D);
/*Set my serial port to same as Arduino, baud rate 9600*/
myPort = new Serial(this, Serial.list()[0], 9600); // if you have only ONE COM port active
//myPort = new Serial(this, "COM5", 9600); // if you know the 101 COM port
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
translate(width/2, height/2); // set position to centre
pushMatrix(); // begin object
rotateX(pitch); // RotateX pitch value
rotateY(-yaw); // yaw
rotateZ(-roll); // roll
drawArduino(); // function to draw rough Arduino shape
popMatrix(); // end of object
// Print values to console
print(pitch);
print("\t");
print(roll);
print("\t");
print(-yaw);
println("\t");
}
void serialEvent()
{
message = myPort.readStringUntil(newLine); // read from port until new line (ASCII code 13)
if (message != null) {
ypr = split(message, ","); // split message by commas and store
in String array
yaw = float(ypr[0]); // convert to float yaw
pitch = float(ypr[1]); // convert to float pitch
Instruction
roll = float(ypr[2]); // convert to float roll
}
}
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
}