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