Tag Archives: Control System

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, 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: Control System

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

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!