Tag Archives: IMU

In this Lesson we begin to work on developing a tilt stabilized platform using the BNO055 9-axis sensor, and we will take advantage of all the learning that happened in the first 22 lessons. Now though, we will be moving out of the virtual world of Vpython, and will begin working in the real world. In this lesson we focus on getting the gear together. You can go ahead and order your gear, and then next week we will begin assembling and coding. In addition to the arduino nano, and the BNO055, you will need:

You Will Need Two of These HiTEC Servos

NOTE: I am no longer recommending the MG995 four pack of servos, as I have recently gotten several bad batches, so have moved to the HiTEC linked above.

A set of Pan Tilt Brackets:

PC Board Power Supply (If you have the ELEGOO Kit, you already have this)

Good Wall Wart Power Plug for the Power Supply (if you dont have one)

Bundle of Extra Cables

OK, get your gear ordered and we will start putting things together next week.

Arduino, Arduino 9-Axis IMU Totorials

9-Axis IMU LESSON 21: Visualizing 3D Rotations in Vpython using Quaternions

January 2, 2020 admin

In this lesson we show how to use quaternions from the BNO055 to create a visualization in Vpython. The visualization is a complete 3D free body rotation of a rigid body. To build this project you will need an Arduino Nano, and an Adafruit BNO055 Inertial Measurement Sensor.

This is the code we developed in the video posted here for your convenience. This code is for demo purposes only and should not be used in real applications. It is for educational purposes only.

// ====================================================================
// DISCLAIMER:
// This code is provided as-is for educational and experimental 
// purposes only. The author makes no representations or warranties of 
// any kind concerning the safety, suitability, or accuracy of this 
// code. Use at your own risk. The author assumes no liability for any 
// damages, system failures, security breaches, or network issues 
// resulting from the use or implementation of this script.
// ====================================================================

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
myIMU.begin();
delay(1000);
int8_t temp=myIMU.getTemp();
myIMU.setExtCrystalUse(true);
}

void loop() {
  // put your main code here, to run repeatedly:
uint8_t system, gyro, accel, mg = 0;
myIMU.getCalibration(&system, &gyro, &accel, &mg);

imu::Quaternion quat=myIMU.getQuat();

Serial.print(quat.w());
Serial.print(",");
Serial.print(quat.x());
Serial.print(",");
Serial.print(quat.y());
Serial.print(",");
Serial.print(quat.z());
Serial.print(",");
Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.println(system);

delay(BNO055_SAMPLERATE_DELAY_MS);
}

// ====================================================================

// DISCLAIMER:

// This code is provided as-is for educational and experimental

// purposes only. The author makes no representations or warranties of

// any kind concerning the safety, suitability, or accuracy of this

// code. Use at your own risk. The author assumes no liability for any

// damages, system failures, security breaches, or network issues

// resulting from the use or implementation of this script.

// ====================================================================

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <math.h>

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {

// put your setup code here, to run once:

Serial.begin(115200);

myIMU.begin();

delay(1000);

int8_t temp=myIMU.getTemp();

myIMU.setExtCrystalUse(true);

}

void loop() {

// put your main code here, to run repeatedly:

uint8_t system, gyro, accel, mg = 0;

myIMU.getCalibration(&system, &gyro, &accel, &mg);

imu::Quaternion quat=myIMU.getQuat();

Serial.print(quat.w());

Serial.print(",");

Serial.print(quat.x());

Serial.print(",");

Serial.print(quat.y());

Serial.print(",");

Serial.print(quat.z());

Serial.print(",");

Serial.print(accel);

Serial.print(",");

Serial.print(gyro);

Serial.print(",");

Serial.print(mg);

Serial.print(",");

Serial.println(system);

delay(BNO055_SAMPLERATE_DELAY_MS);

}

This is the code we developed on the python side to do the visualization from the passed quaternions.

# ====================================================================
# DISCLAIMER:
# This code is provided as-is for educational and experimental 
# purposes only. The author makes no representations or warranties of 
# any kind concerning the safety, suitability, or accuracy of this 
# code. Use at your own risk. The author assumes no liability for any 
# damages, system failures, security breaches, or network issues 
# resulting from the use or implementation of this script.
# ====================================================================

from vpython import *
from time import *
import numpy as np
import math
import serial
ad=serial.Serial('com5',115200)
ts=1
sleep(ts)

scene.range=5
scene.background=color.yellow
toRad=2*np.pi/360
toDeg=1/toRad
scene.forward=vector(-1,-1,-1)

scene.width=1200
scene.height=1080

xarrow=arrow(lenght=2, shaftwidth=.1, color=color.red,axis=vector(1,0,0))
yarrow=arrow(lenght=2, shaftwidth=.1, color=color.green,axis=vector(0,1,0))
zarrow=arrow(lenght=4, shaftwidth=.1, color=color.blue,axis=vector(0,0,1))

frontArrow=arrow(length=4,shaftwidth=.1,color=color.purple,axis=vector(1,0,0))
upArrow=arrow(length=1,shaftwidth=.1,color=color.magenta,axis=vector(0,1,0))
sideArrow=arrow(length=2,shaftwidth=.1,color=color.orange,axis=vector(0,0,1))

bBoard=box(length=6,width=2,height=.2,opacity=.8,pos=vector(0,0,0,))
bn=box(length=1,width=.75,height=.1, pos=vector(-.5,.1+.05,0),color=color.blue)
nano=box(lenght=1.75,width=.6,height=.1,pos=vector(-2,.1+.05,0),color=color.green)
myObj=compound([bBoard,bn,nano])
while (True):
    try:
        while (ad.inWaiting()==0):
            pass
        dataPacket=ad.readline()
        dataPacket=str(dataPacket,'utf-8')
        splitPacket=dataPacket.split(",")
        q0=float(splitPacket[0])
        q1=float(splitPacket[1])
        q2=float(splitPacket[2])
        q3=float(splitPacket[3])

        roll=-math.atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2))
        pitch=math.asin(2*(q0*q2-q3*q1))
        yaw=-math.atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3))-np.pi/2

        rate(50)
        k=vector(cos(yaw)*cos(pitch), sin(pitch),sin(yaw)*cos(pitch))
        y=vector(0,1,0)
        s=cross(k,y)
        v=cross(s,k)
        vrot=v*cos(roll)+cross(k,v)*sin(roll)

        frontArrow.axis=k
        sideArrow.axis=cross(k,vrot)
        upArrow.axis=vrot
        myObj.axis=k
        myObj.up=vrot
        sideArrow.length=2
        frontArrow.length=4
        upArrow.length=1
    except:
        pass

# ====================================================================

# DISCLAIMER:

# This code is provided as-is for educational and experimental

# purposes only. The author makes no representations or warranties of

# any kind concerning the safety, suitability, or accuracy of this

# code. Use at your own risk. The author assumes no liability for any

# damages, system failures, security breaches, or network issues

# resulting from the use or implementation of this script.

# ====================================================================

from vpython import *

from time import *

import numpy as np

import math

import serial

ad=serial.Serial('com5',115200)

ts=1

sleep(ts)

scene.range=5

scene.background=color.yellow

toRad=2*np.pi/360

toDeg=1/toRad

scene.forward=vector(-1,-1,-1)

scene.width=1200

scene.height=1080

xarrow=arrow(lenght=2, shaftwidth=.1, color=color.red,axis=vector(1,0,0))

yarrow=arrow(lenght=2, shaftwidth=.1, color=color.green,axis=vector(0,1,0))

zarrow=arrow(lenght=4, shaftwidth=.1, color=color.blue,axis=vector(0,0,1))

frontArrow=arrow(length=4,shaftwidth=.1,color=color.purple,axis=vector(1,0,0))

upArrow=arrow(length=1,shaftwidth=.1,color=color.magenta,axis=vector(0,1,0))

sideArrow=arrow(length=2,shaftwidth=.1,color=color.orange,axis=vector(0,0,1))

bBoard=box(length=6,width=2,height=.2,opacity=.8,pos=vector(0,0,0,))

bn=box(length=1,width=.75,height=.1, pos=vector(-.5,.1+.05,0),color=color.blue)

nano=box(lenght=1.75,width=.6,height=.1,pos=vector(-2,.1+.05,0),color=color.green)

myObj=compound([bBoard,bn,nano])

while (True):

try:

while (ad.inWaiting()==0):

pass

dataPacket=ad.readline()

dataPacket=str(dataPacket,'utf-8')

splitPacket=dataPacket.split(",")

q0=float(splitPacket[0])

q1=float(splitPacket[1])

q2=float(splitPacket[2])

q3=float(splitPacket[3])

roll=-math.atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2))

pitch=math.asin(2*(q0*q2-q3*q1))

yaw=-math.atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3))-np.pi/2

rate(50)

k=vector(cos(yaw)*cos(pitch), sin(pitch),sin(yaw)*cos(pitch))

y=vector(0,1,0)

s=cross(k,y)

v=cross(s,k)

vrot=v*cos(roll)+cross(k,v)*sin(roll)

frontArrow.axis=k

sideArrow.axis=cross(k,vrot)

upArrow.axis=vrot

myObj.axis=k

myObj.up=vrot

sideArrow.length=2

frontArrow.length=4

upArrow.length=1

except:

pass

Arduino, Arduino 9-Axis IMU Totorials

9-Axis IMU LESSON 20: Vpython Visualization of Roll, Pitch, and Yaw

December 26, 2019 admin

<span data-mce-type="bookmark" style="display: inline-block; width: 0px; overflow: hidden; line-height: 0;" class="mce_SELRES_start"></span>
This is the arduino code we developed in this lesson to approximate roll, pitch and yaw over small ranges.

// ====================================================================
// DISCLAIMER:
// This code is provided as-is for educational and experimental 
// purposes only. The author makes no representations or warranties of 
// any kind concerning the safety, suitability, or accuracy of this 
// code. Use at your own risk. The author assumes no liability for any 
// damages, system failures, security breaches, or network issues 
// resulting from the use or implementation of this script.
// ====================================================================

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>
float thetaM;
float phiM;
float thetaFold=0;
float thetaFnew;
float phiFold=0;
float phiFnew;

float thetaG=0;
float phiG=0;

float theta;
float phi;

float thetaRad;
float phiRad;

float Xm;
float Ym;
float psi;


float dt;
unsigned long millisOld;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
myIMU.begin();
delay(1000);
int8_t temp=myIMU.getTemp();
myIMU.setExtCrystalUse(true);
millisOld=millis();
}

void loop() {
  // put your main code here, to run repeatedly:
uint8_t system, gyro, accel, mg = 0;
myIMU.getCalibration(&system, &gyro, &accel, &mg);
imu::Vector<3> acc =myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
imu::Vector<3> gyr =myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
imu::Vector<3> mag =myIMU.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
thetaM=-atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;
phiM=-atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;
phiFnew=.95*phiFold+.05*phiM;
thetaFnew=.95*thetaFold+.05*thetaM;

dt=(millis()-millisOld)/1000.;
millisOld=millis();
theta=(theta+gyr.y()*dt)*.95+thetaM*.05;
phi=(phi-gyr.x()*dt)*.95+ phiM*.05;
thetaG=thetaG+gyr.y()*dt;
phiG=phiG-gyr.x()*dt;

phiRad=phi/360*(2*3.14);
thetaRad=theta/360*(2*3.14);

Xm=mag.x()*cos(thetaRad)-mag.y()*sin(phiRad)*sin(thetaRad)+mag.z()*cos(phiRad)*sin(thetaRad);
Ym=mag.y()*cos(phiRad)+mag.z()*sin(phiRad);

psi=atan2(Ym,Xm)/(2*3.14)*360;

Serial.print(phi);
Serial.print(",");
Serial.print(theta);
Serial.print(",");
Serial.print(psi);
Serial.print(",");
Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.println(system);


phiFold=phiFnew;
thetaFold=thetaFnew;

 
delay(BNO055_SAMPLERATE_DELAY_MS);
}

100

101

// ====================================================================

// DISCLAIMER:

// This code is provided as-is for educational and experimental

// purposes only. The author makes no representations or warranties of

// any kind concerning the safety, suitability, or accuracy of this

// code. Use at your own risk. The author assumes no liability for any

// damages, system failures, security breaches, or network issues

// resulting from the use or implementation of this script.

// ====================================================================

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <math.h>

float thetaM;

float phiM;

float thetaFold=0;

float thetaFnew;

float phiFold=0;

float phiFnew;

float thetaG=0;

float phiG=0;

float theta;

float phi;

float thetaRad;

float phiRad;

float Xm;

float Ym;

float psi;

float dt;

unsigned long millisOld;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {

// put your setup code here, to run once:

Serial.begin(115200);

myIMU.begin();

delay(1000);

int8_t temp=myIMU.getTemp();

myIMU.setExtCrystalUse(true);

millisOld=millis();

}

void loop() {

// put your main code here, to run repeatedly:

uint8_t system, gyro, accel, mg = 0;

myIMU.getCalibration(&system, &gyro, &accel, &mg);

imu::Vector<3> acc =myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);

imu::Vector<3> gyr =myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

imu::Vector<3> mag =myIMU.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);

thetaM=-atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;

phiM=-atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;

phiFnew=.95*phiFold+.05*phiM;

thetaFnew=.95*thetaFold+.05*thetaM;

dt=(millis()-millisOld)/1000.;

millisOld=millis();

theta=(theta+gyr.y()*dt)*.95+thetaM*.05;

phi=(phi-gyr.x()*dt)*.95+ phiM*.05;

thetaG=thetaG+gyr.y()*dt;

phiG=phiG-gyr.x()*dt;

phiRad=phi/360*(2*3.14);

thetaRad=theta/360*(2*3.14);

Xm=mag.x()*cos(thetaRad)-mag.y()*sin(phiRad)*sin(thetaRad)+mag.z()*cos(phiRad)*sin(thetaRad);

Ym=mag.y()*cos(phiRad)+mag.z()*sin(phiRad);

psi=atan2(Ym,Xm)/(2*3.14)*360;

Serial.print(phi);

Serial.print(",");

Serial.print(theta);

Serial.print(",");

Serial.print(psi);

Serial.print(",");

Serial.print(accel);

Serial.print(",");

Serial.print(gyro);

Serial.print(",");

Serial.print(mg);

Serial.print(",");

Serial.println(system);

phiFold=phiFnew;

thetaFold=thetaFnew;

delay(BNO055_SAMPLERATE_DELAY_MS);

}

This is the python code we developed to visualize the 3 dimensional rotation of a rigid body.

# ====================================================================
# DISCLAIMER:
# This code is provided as-is for educational and experimental 
# purposes only. The author makes no representations or warranties of 
# any kind concerning the safety, suitability, or accuracy of this 
# code. Use at your own risk. The author assumes no liability for any 
# damages, system failures, security breaches, or network issues 
# resulting from the use or implementation of this script.
# ====================================================================

from vpython import *
from time import *
import numpy as np
import math
import serial
ad=serial.Serial('com5',115200)
ts=1
sleep(ts)

scene.range=5
toRad=2*np.pi/360
toDeg=1/toRad
scene.forward=vector(-1,-1,-1)

scene.width=600
scene.height=600

xarrow=arrow(lenght=2, shaftwidth=.1, color=color.red,axis=vector(1,0,0))
yarrow=arrow(lenght=2, shaftwidth=.1, color=color.green,axis=vector(0,1,0))
zarrow=arrow(lenght=4, shaftwidth=.1, color=color.blue,axis=vector(0,0,1))

frontArrow=arrow(length=4,shaftwidth=.1,color=color.purple,axis=vector(1,0,0))
upArrow=arrow(length=1,shaftwidth=.1,color=color.magenta,axis=vector(0,1,0))
sideArrow=arrow(length=2,shaftwidth=.1,color=color.orange,axis=vector(0,0,1))

bBoard=box(length=6,width=2,height=.2,opacity=.8,pos=vector(0,0,0,))
bn=box(length=1,width=.75,height=.1, pos=vector(-.5,.1+.05,0),color=color.blue)
nano=box(lenght=1.75,width=.6,height=.1,pos=vector(-2,.1+.05,0),color=color.green)
myObj=compound([bBoard,bn,nano])
while (True):
    while (ad.inWaiting()==0):
        pass
    dataPacket=ad.readline()
    dataPacket=str(dataPacket,'utf-8')
    splitPacket=dataPacket.split(",")
    roll=float(splitPacket[0])*toRad
    pitch=float(splitPacket[1])*toRad
    yaw=float(splitPacket[2])*toRad+np.pi
    print("Roll=",roll*toDeg," Pitch=",pitch*toDeg,"Yaw=",yaw*toDeg)
    rate(50)
    k=vector(cos(yaw)*cos(pitch), sin(pitch),sin(yaw)*cos(pitch))
    y=vector(0,1,0)
    s=cross(k,y)
    v=cross(s,k)
    vrot=v*cos(roll)+cross(k,v)*sin(roll)

    frontArrow.axis=k
    sideArrow.axis=cross(k,vrot)
    upArrow.axis=vrot
    myObj.axis=k
    myObj.up=vrot
    sideArrow.length=2
    frontArrow.length=4
    upArrow.length=1

# ====================================================================

# DISCLAIMER:

# This code is provided as-is for educational and experimental

# purposes only. The author makes no representations or warranties of

# any kind concerning the safety, suitability, or accuracy of this

# code. Use at your own risk. The author assumes no liability for any

# damages, system failures, security breaches, or network issues

# resulting from the use or implementation of this script.

# ====================================================================

from vpython import *

from time import *

import numpy as np

import math

import serial

ad=serial.Serial('com5',115200)

ts=1

sleep(ts)

scene.range=5

toRad=2*np.pi/360

toDeg=1/toRad

scene.forward=vector(-1,-1,-1)

scene.width=600

scene.height=600