Category Archives: Arduino 9-Axis IMU Totorials

In this video lesson we show how to create a tilt compensated digital compass. Calculating heading based simply on the measured magnetometer values in the X and Y directions only works accurately when the compass is sitting flat, or horizontal with the earth’s surface. If we introduce a tilt, either by applying pitch or roll to the system, calculated heading, or yaw will no longer be accurate. In the video above, we show you how to mathematically ‘un-tilt’ the sensor to get accurate heading readings when the device is not perfectly flat.

We are working with a GY-87 9-axis IMU, and an Arduino Uno R4 WiFi. Below is the schematic we are using in this project:

MPU6050 — Schematic for connecting the GY-87 module to the Arduino

For your convenience, the code developed in this video lesson is included below. Please notice that the calibration constants in the code below are for my GY-87 module. You need to calibrate your own module, as my numbers below would likely be different from your numbers. We showed how to do the calibration in THIS LESSON.

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <QMC5883LCompass.h>
float AxRaw,AyRaw,AzRaw,GxRaw,GyRaw,GzRaw,MxRaw,MyRaw,MzRaw;
float AxCal,AyCal,AzCal,GxCal,GyCal,GzCal,MxCal,MyCal,MzCal;
float rollA,pitchA,yawM,rollC,pitchC,yawC;
float rollG = 0;
float pitchG =0 ;
float yawG = 0;
float deltaRoll = 0;
float deltaPitch = 0;
float deltaYaw = 0;
float alpha = .1;
float rollCrad;
float pitchCrad;

float MxCalH; 
float MyCalH;
int tStart =millis();
Adafruit_MPU6050 mpu;
QMC5883LCompass compass;

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
mpu.begin();
mpu.setI2CBypass(true);
compass.init();
mpu.setGyroRange(MPU6050_RANGE_1000_DEG);
delay(100);
}

void calibrateSensors() {
    const float axOffset = 0.45407887789180545;
    const float ayOffset = 0.09432915233553185;
    const float azOffset = -0.4614157209191969;
    const float axScale = 0.10141341897341144;
    const float ayScale = 0.10141341897341144;
    const float azScale = 0.10141341897341144;
    const float gxOffset = -2.277507235645022;
    const float gyOffset = 0.8794902155258137;
    const float gzOffset = -0.5729577951308232;
    const float mxOffset = -103.48885017294003;
    const float myOffset = -96.80085481152321;
    const float mzOffset = 923.8042898128733;
    const float mxScale = 0.001156333898601669;
    const float myScale = 0.001012882116988885;
    const float mzScale = 0.0009400860982065997;
 
    AxCal = (AxRaw - axOffset) * axScale;
    AyCal = (AyRaw - ayOffset) * ayScale;
    AzCal = (AzRaw - azOffset) * azScale;
    GxCal = GxRaw*180/PI - gxOffset;
    GyCal = GyRaw*180/PI - gyOffset;
    GzCal = GzRaw*180/PI - gzOffset;
    MxCal = (MxRaw - mxOffset) * mxScale;
    MyCal = (MyRaw - myOffset) * myScale;
    MzCal = (MzRaw - mzOffset) * mzScale;
}

void loop() {
  // put your main code here, to run repeatedly:
compass.read();
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);

AxRaw = a.acceleration.x;
AyRaw = a.acceleration.y;
AzRaw = a.acceleration.z;

GxRaw = g.gyro.x;
GyRaw = g.gyro.y;
GzRaw = g.gyro.z;

MxRaw= compass.getX();
MyRaw= compass.getY();
MzRaw= compass.getZ();

calibrateSensors();

rollA = atan2(AyCal,sqrt(AzCal*AzCal + AxCal*AxCal))*180/PI;
pitchA = atan2(AxCal,sqrt(AzCal*AzCal + AyCal*AyCal))*180/PI;

deltaRoll = GxCal*(millis()-tStart)/1000.;
deltaPitch = -GyCal*(millis()-tStart)/1000.;
deltaYaw = -GzCal*(millis()-tStart)/1000.;
tStart = millis();

rollG = rollG + deltaRoll;
pitchG = pitchG + deltaPitch;
yawG = yawG + deltaYaw;

rollC = alpha*(rollA) + (1-alpha)*(rollC+deltaRoll);
pitchC = alpha*(pitchA) + (1-alpha)*(pitchC+deltaPitch);

rollCrad = rollC*PI/180;
pitchCrad = pitchC*PI/180;

MxCalH = MxCal*cos(pitchCrad) -MyCal*sin(rollCrad)*sin(pitchCrad)-MzCal*cos(rollCrad)*sin(pitchCrad);
MyCalH = MyCal*cos(rollCrad) - MzCal*sin(rollCrad);

yawM = atan2(MyCalH,MxCalH)*180./PI;
yawC = alpha*(yawM) + (1-alpha)*(yawC + deltaYaw);

// Serial.print("yawC:");Serial.print(yawC);Serial.print(',');
// Serial.print("yawM:");Serial.print(yawM);Serial.print(',');
// Serial.print("yawG:");Serial.print(yawG);Serial.print(',');
// Serial.print("LL:");Serial.print(-90);Serial.print(',');
// Serial.print("UL:");Serial.println(90);

Serial.print("rollC:");Serial.print(rollC);Serial.print(',');
Serial.print("pitchC:");Serial.print(pitchC);Serial.print(',');
Serial.print("yawC:");Serial.print(yawC);Serial.print(',');
Serial.print("LL:");Serial.print(-90);Serial.print(',');
Serial.print("UL:");Serial.println(90);

delay(100);
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

#include <Adafruit_MPU6050.h>

#include <Adafruit_Sensor.h>

#include <Wire.h>

#include <QMC5883LCompass.h>

float AxRaw,AyRaw,AzRaw,GxRaw,GyRaw,GzRaw,MxRaw,MyRaw,MzRaw;

float AxCal,AyCal,AzCal,GxCal,GyCal,GzCal,MxCal,MyCal,MzCal;

float rollA,pitchA,yawM,rollC,pitchC,yawC;

float rollG = 0;

float pitchG =0 ;

float yawG = 0;

float deltaRoll = 0;

float deltaPitch = 0;

float deltaYaw = 0;

float alpha = .1;

float rollCrad;

float pitchCrad;

float MxCalH;

float MyCalH;

int tStart =millis();

Adafruit_MPU6050 mpu;

QMC5883LCompass compass;

void setup() {

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

Serial.begin(115200);

mpu.begin();

mpu.setI2CBypass(true);

compass.init();

mpu.setGyroRange(MPU6050_RANGE_1000_DEG);

delay(100);

}

void calibrateSensors() {

const float axOffset = 0.45407887789180545;

const float ayOffset = 0.09432915233553185;

const float azOffset = -0.4614157209191969;

const float axScale = 0.10141341897341144;

const float ayScale = 0.10141341897341144;

const float azScale = 0.10141341897341144;

const float gxOffset = -2.277507235645022;

const float gyOffset = 0.8794902155258137;

const float gzOffset = -0.5729577951308232;

const float mxOffset = -103.48885017294003;

const float myOffset = -96.80085481152321;

const float mzOffset = 923.8042898128733;

const float mxScale = 0.001156333898601669;

const float myScale = 0.001012882116988885;

const float mzScale = 0.0009400860982065997;

AxCal = (AxRaw - axOffset) * axScale;

AyCal = (AyRaw - ayOffset) * ayScale;

AzCal = (AzRaw - azOffset) * azScale;

GxCal = GxRaw*180/PI - gxOffset;

GyCal = GyRaw*180/PI - gyOffset;

GzCal = GzRaw*180/PI - gzOffset;

MxCal = (MxRaw - mxOffset) * mxScale;

MyCal = (MyRaw - myOffset) * myScale;

MzCal = (MzRaw - mzOffset) * mzScale;

}

void loop() {

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

compass.read();

sensors_event_t a, g, temp;

mpu.getEvent(&a, &g, &temp);

AxRaw = a.acceleration.x;

AyRaw = a.acceleration.y;

AzRaw = a.acceleration.z;

GxRaw = g.gyro.x;

GyRaw = g.gyro.y;

GzRaw = g.gyro.z;

MxRaw= compass.getX();

MyRaw= compass.getY();

MzRaw= compass.getZ();

calibrateSensors();

rollA = atan2(AyCal,sqrt(AzCal*AzCal + AxCal*AxCal))*180/PI;

pitchA = atan2(AxCal,sqrt(AzCal*AzCal + AyCal*AyCal))*180/PI;

deltaRoll = GxCal*(millis()-tStart)/1000.;

deltaPitch = -GyCal*(millis()-tStart)/1000.;

deltaYaw = -GzCal*(millis()-tStart)/1000.;

tStart = millis();

rollG = rollG + deltaRoll;

pitchG = pitchG + deltaPitch;

yawG = yawG + deltaYaw;

rollC = alpha*(rollA) + (1-alpha)*(rollC+deltaRoll);

pitchC = alpha*(pitchA) + (1-alpha)*(pitchC+deltaPitch);

rollCrad = rollC*PI/180;

pitchCrad = pitchC*PI/180;

MxCalH = MxCal*cos(pitchCrad) -MyCal*sin(rollCrad)*sin(pitchCrad)-MzCal*cos(rollCrad)*sin(pitchCrad);

MyCalH = MyCal*cos(rollCrad) - MzCal*sin(rollCrad);

yawM = atan2(MyCalH,MxCalH)*180./PI;

yawC = alpha*(yawM) + (1-alpha)*(yawC + deltaYaw);

// Serial.print("yawC:");Serial.print(yawC);Serial.print(',');

// Serial.print("yawM:");Serial.print(yawM);Serial.print(',');

// Serial.print("yawG:");Serial.print(yawG);Serial.print(',');

// Serial.print("LL:");Serial.print(-90);Serial.print(',');

// Serial.print("UL:");Serial.println(90);

Serial.print("rollC:");Serial.print(rollC);Serial.print(',');

Serial.print("pitchC:");Serial.print(pitchC);Serial.print(',');

Serial.print("yawC:");Serial.print(yawC);Serial.print(',');

Serial.print("LL:");Serial.print(-90);Serial.print(',');

Serial.print("UL:");Serial.println(90);

delay(100);

}

Arduino 9-Axis IMU Totorials

9-Axis IMU LESSON 26: Understanding PID Control systems with Arduino

February 6, 2020 admin

In this lesson we use our BNO055 9-axis sensor, and our pan-tilt servo mount to create a self-leveling platform based on a classic PID control system. The video takes you step by step through the theory behind a PID controller, and then demonstrates a practical example in hardware. In the video we develop the code below.

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

Servo pitchServo;
Servo rollServo;

float q0;
float q1;
float q2;
float q3;

float k1=.5;
float k2=55;
float k3=.00001;

int milliOld;
int milliNew;
int dt;

float rollTarget=0;
float rollActual;
float rollError=0;
float rollErrorOld;
float rollErrorChange;
float rollErrorSlope=0;
float rollErrorArea=0;
float rollServoVal=90;

float pitchTarget=0;
float pitchActual;
float pitchError=0;
float pitchErrorOld;
float pitchErrorChange;
float pitchErrorSlope=0;
float pitchErrorArea=0;
float pitchServoVal=90;

#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);
rollServo.attach(2);
pitchServo.attach(3);

rollServo.write(rollServoVal);
delay(20);
pitchServo.write(pitchServoVal);
delay(20);

milliNew=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::Quaternion quat=myIMU.getQuat();

q0=quat.w();
q1=quat.x();
q2=quat.y();
q3=quat.z();

rollActual=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
pitchActual=asin(2*(q0*q2-q3*q1));

rollActual=rollActual/(2*3.141592654)*360;
pitchActual=pitchActual/(2*3.141592654)*360;

milliOld=milliNew;
milliNew=millis();
dt=milliNew-milliOld;

rollErrorOld=rollError;
rollError=rollTarget-rollActual;
rollErrorChange=rollError-rollErrorOld;
rollErrorSlope=rollErrorChange/dt;
rollErrorArea=rollErrorArea+rollError*dt;

pitchErrorOld=pitchError;
pitchError=pitchTarget-pitchActual;
pitchErrorChange=pitchError-pitchErrorOld;
pitchErrorSlope=pitchErrorChange/dt;
pitchErrorArea=pitchErrorArea+pitchError*dt;

rollServoVal=rollServoVal+k1*rollError+k2*rollErrorSlope+k3*rollErrorArea;
rollServo.write(rollServoVal);

pitchServoVal=pitchServoVal+k1*pitchError+k2*pitchErrorSlope+k3*pitchErrorArea;
pitchServo.write(pitchServoVal);

Serial.print(rollTarget);
Serial.print(",");
Serial.print(rollActual);
Serial.print(",");
Serial.print(pitchTarget);
Serial.print(",");
Serial.print(pitchActual);
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);
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <math.h>

#include <Servo.h>

Servo pitchServo;

Servo rollServo;

float q0;

float q1;

float q2;

float q3;

float k1=.5;

float k2=55;

float k3=.00001;

int milliOld;

int milliNew;

int dt;

float rollTarget=0;

float rollActual;

float rollError=0;

float rollErrorOld;

float rollErrorChange;

float rollErrorSlope=0;

float rollErrorArea=0;

float rollServoVal=90;

float pitchTarget=0;

float pitchActual;

float pitchError=0;

float pitchErrorOld;

float pitchErrorChange;

float pitchErrorSlope=0;

float pitchErrorArea=0;

float pitchServoVal=90;

#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);

rollServo.attach(2);

pitchServo.attach(3);

rollServo.write(rollServoVal);

delay(20);

pitchServo.write(pitchServoVal);

delay(20);

milliNew=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::Quaternion quat=myIMU.getQuat();

q0=quat.w();

q1=quat.x();

q2=quat.y();

q3=quat.z();

rollActual=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));

pitchActual=asin(2*(q0*q2-q3*q1));

rollActual=rollActual/(2*3.141592654)*360;

pitchActual=pitchActual/(2*3.141592654)*360;

milliOld=milliNew;

milliNew=millis();

dt=milliNew-milliOld;

rollErrorOld=rollError;

rollError=rollTarget-rollActual;

rollErrorChange=rollError-rollErrorOld;

rollErrorSlope=rollErrorChange/dt;

rollErrorArea=rollErrorArea+rollError*dt;

pitchErrorOld=pitchError;

pitchError=pitchTarget-pitchActual;

pitchErrorChange=pitchError-pitchErrorOld;

pitchErrorSlope=pitchErrorChange/dt;

pitchErrorArea=pitchErrorArea+pitchError*dt;

rollServoVal=rollServoVal+k1*rollError+k2*rollErrorSlope+k3*rollErrorArea;

rollServo.write(rollServoVal);

pitchServoVal=pitchServoVal+k1*pitchError+k2*pitchErrorSlope+k3*pitchErrorArea;

pitchServo.write(pitchServoVal);

Serial.print(rollTarget);

Serial.print(",");

Serial.print(rollActual);

Serial.print(",");

Serial.print(pitchTarget);

Serial.print(",");

Serial.print(pitchActual);

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);

}

Arduino 9-Axis IMU Totorials

9-Axis IMU LESSON 25: Proportional Control System for Self Leveling Platform

January 30, 2020 admin

In this lesson we improve our earlier control system for our self leveling platform. In our earlier work, our control system would elliminate system error by constant corrections of 1 degree each time through the loop. In this lesson, we show how to feedback a control signal that is proportional to the error. Hence larger errors will get a larger correction signal, and the error is driven to zero much quicker.

This is the code we developed during this lesson:

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

Servo pitchServo;
Servo rollServo;

float q0;
float q1;
float q2;
float q3;

float rollTarget=0;
float rollActual;
float rollError;
float rollServoVal=90;

float pitchTarget=0;
float pitchActual;
float pitchError;
float pitchServoVal=90;

#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);
rollServo.attach(2);
pitchServo.attach(3);

rollServo.write(rollServoVal);
delay(20);
pitchServo.write(pitchServoVal);
delay(20);
}

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();

q0=quat.w();
q1=quat.x();
q2=quat.y();
q3=quat.z();

rollActual=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
pitchActual=asin(2*(q0*q2-q3*q1));

rollActual=rollActual/(2*3.141592654)*360;
pitchActual=pitchActual/(2*3.141592654)*360;

rollError=rollTarget-rollActual;
pitchError=pitchTarget-pitchActual;

if (abs(pitchError)>1.5){
  pitchServoVal=pitchServoVal+pitchError/2;
  pitchServo.write(pitchServoVal);
  delay(20);
}


if (abs(rollError)>1.5){
  rollServoVal=rollServoVal+rollError/2;
  rollServo.write(rollServoVal);
  delay(20);
}

Serial.print(rollTarget);
Serial.print(",");
Serial.print(rollActual);
Serial.print(",");
Serial.print(pitchTarget);
Serial.print(",");
Serial.print(pitchActual);
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);
}

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <math.h>

#include <Servo.h>

Servo pitchServo;

Servo rollServo;

float q0;

float q1;

float q2;

float q3;

float rollTarget=0;

float rollActual;

float rollError;

float rollServoVal=90;

float pitchTarget=0;

float pitchActual;

float pitchError;

float pitchServoVal=90;

#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);

rollServo.attach(2);

pitchServo.attach(3);

rollServo.write(rollServoVal);

delay(20);

pitchServo.write(pitchServoVal);

delay(20);

}

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();

q0=quat.w();

q1=quat.x();

q2=quat.y();

q3=quat.z();

rollActual=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));

pitchActual=asin(2*(q0*q2-q3*q1));

rollActual=rollActual/(2*3.141592654)*360;

pitchActual=pitchActual/(2*3.141592654)*360;

rollError=rollTarget-rollActual;

pitchError=pitchTarget-pitchActual;

if (abs(pitchError)>1.5){

pitchServoVal=pitchServoVal+pitchError/2;

pitchServo.write(pitchServoVal);

delay(20);

}

if (abs(rollError)>1.5){

rollServoVal=rollServoVal+rollError/2;

rollServo.write(rollServoVal);

delay(20);

}

Serial.print(rollTarget);

Serial.print(",");

Serial.print(rollActual);

Serial.print(",");

Serial.print(pitchTarget);

Serial.print(",");

Serial.print(pitchActual);

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);

}

Arduino, Arduino 9-Axis IMU Totorials, Tutorial

9-Axis IMU LESSON 24: How To Build a Self Leveling Platform with Arduino

January 23, 2020 admin

In this lesson we develop the initial simple code for leveling the platform. We implement the simplest control system, which is to just increment or decrement the servos based on whether the actual position is greater or less than the target position. We increment/decrement by one degree, since this is the smallest allowed change in servo position. Next week we will implement a more sophisticated control system. Below is the arduino code developed in this lesson.

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

Servo pitchServo;
Servo rollServo;

float q0;
float q1;
float q2;
float q3;

float rollTarget=0;
float rollActual;
float rollError;
float rollServoVal=90;

float pitchTarget=0;
float pitchActual;
float pitchError;
float pitchServoVal=90;

#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);
rollServo.attach(2);
pitchServo.attach(3);

rollServo.write(rollServoVal);
delay(20);
pitchServo.write(pitchServoVal);
delay(20);
}

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();

q0=quat.w();
q1=quat.x();
q2=quat.y();
q3=quat.z();

rollActual=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
pitchActual=asin(2*(q0*q2-q3*q1));

rollActual=rollActual/(2*3.141592654)*360;
pitchActual=pitchActual/(2*3.141592654)*360;

rollError=rollTarget-rollActual;
pitchError=pitchTarget-pitchActual;

if (pitchError>1.5){
  pitchServoVal=pitchServoVal+1;
  pitchServo.write(pitchServoVal);
  delay(20);
}
if (pitchError<-1.5){
  pitchServoVal=pitchServoVal-1;
  pitchServo.write(pitchServoVal);
  delay(20);
}

if (rollError>1.5){
  rollServoVal=rollServoVal+1;
  rollServo.write(rollServoVal);
  delay(20);
}
if (rollError<-1.5){
  rollServoVal=rollServoVal-1;
  rollServo.write(rollServoVal);
  delay(20);
}

Serial.print(rollTarget);
Serial.print(",");
Serial.print(rollActual);
Serial.print(",");
Serial.print(pitchTarget);
Serial.print(",");
Serial.print(pitchActual);
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);
}

100

101

102

103

104

105

106

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <math.h>

#include <Servo.h>

Servo pitchServo;

Servo rollServo;

float q0;

float q1;

float q2;

float q3;

float rollTarget=0;

float rollActual;

float rollError;

float rollServoVal=90;

float pitchTarget=0;

float pitchActual;

float pitchError;

float pitchServoVal=90;

#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);

rollServo.attach(2);

pitchServo.attach(3);

rollServo.write(rollServoVal);

delay(20);

pitchServo.write(pitchServoVal);

delay(20);

}

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();

q0=quat.w();

q1=quat.x();

q2=quat.y();

q3=quat.z();

rollActual=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));

pitchActual=asin(2*(q0*q2-q3*q1));

rollActual=rollActual/(2*3.141592654)*360;

pitchActual=pitchActual/(2*3.141592654)*360;

rollError=rollTarget-rollActual;

pitchError=pitchTarget-pitchActual;

if (pitchError>1.5){

pitchServoVal=pitchServoVal+1;

pitchServo.write(pitchServoVal);

delay(20);

}

if (pitchError<-1.5){

pitchServoVal=pitchServoVal-1;

pitchServo.write(pitchServoVal);

delay(20);

}

if (rollError>1.5){

rollServoVal=rollServoVal+1;

rollServo.write(rollServoVal);

delay(20);

}

if (rollError<-1.5){

rollServoVal=rollServoVal-1;

rollServo.write(rollServoVal);

delay(20);

}

Serial.print(rollTarget);

Serial.print(",");

Serial.print(rollActual);

Serial.print(",");

Serial.print(pitchTarget);

Serial.print(",");

Serial.print(pitchActual);

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);

}

Arduino 9-Axis IMU Totorials

9-Axis IMU LESSON 23: Self Leveling Platform Using BNO055 and Arduino Controlled Servos

January 16, 2020 admin

<span data-mce-type="bookmark" style="display: inline-block; width: 0px; overflow: hidden; line-height: 0;" class="mce_SELRES_start"></span>In this lesson we connect the circuit and do the mechanical build for our self leveling platform. You can see a list of the gear we are using in LESSON 22.

Technology Tutorials

Category Archives: Arduino 9-Axis IMU Totorials

Tilt Compensated Digital Compass

9-Axis IMU LESSON 26: Understanding PID Control systems with Arduino

9-Axis IMU LESSON 25: Proportional Control System for Self Leveling Platform

9-Axis IMU LESSON 24: How To Build a Self Leveling Platform with Arduino

9-Axis IMU LESSON 23: Self Leveling Platform Using BNO055 and Arduino Controlled Servos

Making The World a Better Place One High Tech Project at a Time. Enjoy!