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.

// ====================================================================
// 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>
#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

122

123

124

125

126

127

128

129

130

131

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

// 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>

#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, Tutorial

Arduino Tutorial 47: Binary and Hexadecimal Bit Flipper

February 4, 2020 admin

In this lesson we explore how to create a Binary or Hexadecimal Bit Flipper. From our earlier lessons you see we can visually represent Hexadecimal or Binary numbers with a series of LED, with an on LED representing a “1” and an off LED representing a “0”. In programming and circuit applications, it is sometimes useful to “flip” or invert the bits. For an 8 bit number, one could do this in a program with 255 IF statements, but there is a simpler way. If you think about it, you can get the flippedByte by simple subtracting the byte from 0xFF, or 0b11111111. If you try some test cases, you can see that this will always work.

Simply stated, flippedByte=0xFF-Byte,

or if you prefer thinking in binary,

flippedByte=0b11111111-Byte

This is the circuit we are using to drive the 8 LED with the 74HX595 chip, and all this was explained in Tutorial 42.

This is the schematic we use in this example to control 8 LEDs from the 74HC595 chip.

This is the code which we developed in the video above.

int latchPin=11;
int clockPin=9;
int dataPin=12;
int dt=1000;

byte myByte=0x18;
byte myByteFlipped;

void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);
pinMode(latchPin,OUTPUT);
pinMode(dataPin,OUTPUT);
pinMode(clockPin,OUTPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
digitalWrite(latchPin,LOW);
shiftOut(dataPin,clockPin,LSBFIRST,myByte);
digitalWrite(latchPin,HIGH);
delay(dt);
myByteFlipped=255-myByte;
digitalWrite(latchPin,LOW);
shiftOut(dataPin,clockPin,LSBFIRST,myByteFlipped);
digitalWrite(latchPin,HIGH);
delay(dt);
}

int latchPin=11;

int clockPin=9;

int dataPin=12;

int dt=1000;

byte myByte=0x18;

byte myByteFlipped;

void setup() {

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

Serial.begin(9600);

pinMode(latchPin,OUTPUT);

pinMode(dataPin,OUTPUT);

pinMode(clockPin,OUTPUT);

}

void loop() {

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

digitalWrite(latchPin,LOW);

shiftOut(dataPin,clockPin,LSBFIRST,myByte);

digitalWrite(latchPin,HIGH);

delay(dt);

myByteFlipped=255-myByte;

digitalWrite(latchPin,LOW);

shiftOut(dataPin,clockPin,LSBFIRST,myByteFlipped);

digitalWrite(latchPin,HIGH);

delay(dt);

}

An official Arduino Uno R3 is available HERE. In this new series of lessons, I will be using the sensor and other components found in this KIT.

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:

// ====================================================================
// 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>
#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);
}

100

101

102

103

104

105

106

107

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

// 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>

#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, Tutorial

Arduino Tutorial 46: HOMEWORK- Create a Binary Bit Flipper with the 74HC595

January 28, 2020 admin

The purpose of this lesson is to assign you homework. Your homework is to create a Bit Flipper. That is, for an 8 bit Binary or Hex number, invert the bits . . . “1” bits should become “0” and “0” bits should become “1”. For example,

if myByte=00001111

the flipped version of this would be

myByteFlipped=11110000

Similarly if myByte=00000001

myByteFlipped=11111110

You could do this with 255 if statements, but see if you can figure out a better way of doing it, and then demonstrate your results using the circuit we have been using in the last few lessons.

An official Arduino Uno R3 is available HERE. In this new series of lessons, I will be using the sensor and other components found in this KIT.

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.

// ====================================================================
// 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>
#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

107

108

109

110

111

112

113

114

115

116

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

// 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>

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

}

Technology Tutorials

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

Arduino Tutorial 47: Binary and Hexadecimal Bit Flipper

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

Arduino Tutorial 46: HOMEWORK- Create a Binary Bit Flipper with the 74HC595

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

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