Jannes De Brabandere / Mbed 2 deprecated gimbalController_brushless_IMU

//

Dependencies:   MPU6050 brushlessMotor mbed

Fork of gimbalController_brushless_IMU by Baser Kandehir

main.cpp@8:c573f315319a, 2016-05-12 (annotated)

Committer:
alfaleader
Date:
Thu May 12 20:10:25 2016 +0000
Revision:
8:c573f315319a
Parent:
7:b65164847018
Child:
9:2779500685cb
//

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BaserK 0:40b56bdec1d2 1 #include "mbed.h"
BaserK 0:40b56bdec1d2 2 #include "MPU6050.h"
alfaleader 8:c573f315319a 3 #include "brushlessMotor.h"
BaserK 0:40b56bdec1d2 4
alfaleader 8:c573f315319a 5 Serial pc(USBTX, USBRX); // Debug serial connectie (voor de computer)
alfaleader 8:c573f315319a 6 MPU6050 mpu6050; // mpu6050 object voor onze gyroscoop
alfaleader 8:c573f315319a 7 Ticker filter; // een Ticker object voor onze filter --> een ticker is een functie die x keer per seconde uitgevoerd gaat worden
alfaleader 8:c573f315319a 8 Ticker gimbal; // ticker voor de gimball
alfaleader 8:c573f315319a 9 Ticker speed; // ticker voor de snelheids controle
BaserK 0:40b56bdec1d2 10
alfaleader 8:c573f315319a 11 //Functie prototypes, zodat het programma weet dat we deze functies later gaan "invullen"
BaserK 0:40b56bdec1d2 12 void compFilter();
BaserK 2:f9f4d36c2367 13 void gimbalPID();
BaserK 5:477eaa33eff5 14 void speedControl();
BaserK 0:40b56bdec1d2 15
alfaleader 8:c573f315319a 16 //variabelen nodig voor ons programma
BaserK 0:40b56bdec1d2 17 float pitchAngle = 0;
BaserK 0:40b56bdec1d2 18 float rollAngle = 0;
alfaleader 8:c573f315319a 19 bool dir; // links of rechts
alfaleader 8:c573f315319a 20 bool stop; // motor moet stoppen?
alfaleader 8:c573f315319a 21 float delay; // delay tussen onze stappen
alfaleader 8:c573f315319a 22
alfaleader 8:c573f315319a 23 //PID
BaserK 5:477eaa33eff5 24 float Kp = 10;
BaserK 6:af164f09f963 25 float Ki = 0.0001;
BaserK 6:af164f09f963 26 float Kd = 5;
alfaleader 8:c573f315319a 27 float set_point = 0; // camera hoek
BaserK 2:f9f4d36c2367 28 float proportional = 0;
BaserK 2:f9f4d36c2367 29 float last_proportional =0;
BaserK 2:f9f4d36c2367 30 float integral = 0;
BaserK 2:f9f4d36c2367 31 float derivative = 0;
alfaleader 8:c573f315319a 32 float errorPID = 0;
BaserK 0:40b56bdec1d2 33
BaserK 1:2ae94169eee6 34 int main()
BaserK 0:40b56bdec1d2 35 {
alfaleader 8:c573f315319a 36 pc.baud(9600); // PC serial connectie maken voor debugging met baud rate 9600
alfaleader 8:c573f315319a 37 mpu6050.whoAmI(); // Communicatie test? Zijn we wel verbonden met de MPU6050 of is er een probleem met de I2C connectie
alfaleader 8:c573f315319a 38 mpu6050.calibrate(accelBias,gyroBias); // Calibreren en upload deze waarden in het MPU6050 bias register, dit is onze 0 waarde waar de camera moet izjn
alfaleader 8:c573f315319a 39 pc.printf("Calibratie compleet \r\n");
alfaleader 8:c573f315319a 40 mpu6050.init(); // Initializeer de gyroscoop
alfaleader 8:c573f315319a 41 pc.printf("MPU6050 is klaar \r\n\r\n");
BaserK 5:477eaa33eff5 42
alfaleader 8:c573f315319a 43 filter.attach(&compFilter, 0.005); // Elke 5ms onze complementary Filter aanroepen
alfaleader 8:c573f315319a 44 gimbal.attach(&gimbalPID, 0.005); // Gimbal aansturing ook elke 5ms aanroepen
alfaleader 8:c573f315319a 45
BaserK 1:2ae94169eee6 46 while(1)
BaserK 6:af164f09f963 47 {
alfaleader 8:c573f315319a 48 pc.printf("Roll: %.1f, Pitch: %.1f\r\n",rollAngle,pitchAngle); //DEBUG
alfaleader 8:c573f315319a 49 wait_ms(500); //500ms wachten
BaserK 1:2ae94169eee6 50 }
BaserK 0:40b56bdec1d2 51 }
BaserK 0:40b56bdec1d2 52
alfaleader 8:c573f315319a 53 //elke 5ms de filter functie van de mpu6050 library aanroepen, berekent de pitch en de roll en voert deze in bij de variabelen pitchAngle en rollAngle
BaserK 2:f9f4d36c2367 54 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
BaserK 2:f9f4d36c2367 55
alfaleader 8:c573f315319a 56 //elke 5ms updaten we ook de gimball
BaserK 2:f9f4d36c2367 57 void gimbalPID()
BaserK 2:f9f4d36c2367 58 {
BaserK 2:f9f4d36c2367 59 proportional = set_point - rollAngle;
BaserK 2:f9f4d36c2367 60 integral += proportional;
BaserK 2:f9f4d36c2367 61 derivative = proportional - last_proportional;
BaserK 2:f9f4d36c2367 62 last_proportional = proportional;
BaserK 2:f9f4d36c2367 63
BaserK 2:f9f4d36c2367 64 errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative);
BaserK 2:f9f4d36c2367 65 (errorPID > 0)?(dir = 1):(dir = 0);
BaserK 5:477eaa33eff5 66
alfaleader 8:c573f315319a 67 // errorPID is restricted between -400 and 400
BaserK 5:477eaa33eff5 68 if(errorPID > 400)
BaserK 5:477eaa33eff5 69 errorPID = 400;
BaserK 5:477eaa33eff5 70 else if(errorPID < -400)
BaserK 5:477eaa33eff5 71 errorPID = -400;
BaserK 3:065a064b3453 72
BaserK 5:477eaa33eff5 73 stop = 0;
alfaleader 8:c573f315319a 74 delay = 0.1/abs(errorPID); // speed should be proportional to error, therefore time delay
alfaleader 8:c573f315319a 75 //between steps should be inverse proportional to error.
BaserK 5:477eaa33eff5 76
BaserK 5:477eaa33eff5 77 if (abs(errorPID)< Kp/2) stop = 1; // 0.5 deg noise margin
BaserK 5:477eaa33eff5 78
BaserK 5:477eaa33eff5 79 if(stop) // if the gimbal is within noise margin, dont move.
BaserK 5:477eaa33eff5 80 speed.detach();
BaserK 5:477eaa33eff5 81 else
alfaleader 8:c573f315319a 82 speed.attach(&speedControl, delay);
BaserK 2:f9f4d36c2367 83 }
BaserK 5:477eaa33eff5 84
BaserK 5:477eaa33eff5 85 void speedControl()
BaserK 5:477eaa33eff5 86 {
BaserK 5:477eaa33eff5 87 brushlessControl(dir, 0);
BaserK 5:477eaa33eff5 88 }