MAJOR LAUNCH: HUSKYLENS 2 6 TOPS LLM MCP AI Vision Sensor is now available. Buy Now →



 
#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
}
 


