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.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 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); } |