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