Tag Archives: IMU

Below we show the detailed schematic we use for the Tilt Meter we are developing based on the MPU6050 IMU sensor.

This is the simple code for measuring acceleration in the x and y axis. We will develop this much further in future lessons, but this is just to verify things are hooked up correctly, and that we can measure accelleration.

from imu import MPU6050
from machine import I2C,Pin
import time

i2c=I2C(0, sda=Pin(16), scl=Pin(17), freq=400000)
mpu = MPU6050(i2c)

while True:
    xAccel=round(mpu.accel.x,1)
    yAccel=round(mpu.accel.y,1)
    print('x: ',xAccel,' G ', 'y: ',yAccel,' G')
    time.sleep(.1)

from imu import MPU6050

from machine import I2C,Pin

import time

i2c=I2C(0, sda=Pin(16), scl=Pin(17), freq=400000)

mpu = MPU6050(i2c)

while True:

xAccel=round(mpu.accel.x,1)

yAccel=round(mpu.accel.y,1)

print('x: ',xAccel,' G ', 'y: ',yAccel,' G')

time.sleep(.1)

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

Tag Archives: IMU

Schematic for Tilt Meter Using the MPU6050

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!