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