![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
//
Dependencies: MPU6050 brushlessMotor mbed
Fork of gimbalController_brushless_IMU by
main.cpp@9:2779500685cb, 2016-05-15 (annotated)
- Committer:
- alfaleader
- Date:
- Sun May 15 16:15:44 2016 +0000
- Revision:
- 9:2779500685cb
- Parent:
- 8:c573f315319a
- Child:
- 10:12f9371f3e0f
f
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
BaserK | 0:40b56bdec1d2 | 1 | #include "mbed.h" |
BaserK | 0:40b56bdec1d2 | 2 | #include "MPU6050.h" |
alfaleader | 9:2779500685cb | 3 | #include "PID.h" |
alfaleader | 8:c573f315319a | 4 | #include "brushlessMotor.h" |
BaserK | 0:40b56bdec1d2 | 5 | |
alfaleader | 8:c573f315319a | 6 | Serial pc(USBTX, USBRX); // Debug serial connectie (voor de computer) |
alfaleader | 8:c573f315319a | 7 | MPU6050 mpu6050; // mpu6050 object voor onze gyroscoop |
alfaleader | 9:2779500685cb | 8 | |
alfaleader | 9:2779500685cb | 9 | PIDControll pitchMotorPID; //PID controle aanmaken voor de pitchmotor |
alfaleader | 9:2779500685cb | 10 | brushlessMotor pitchMotor(D5, D6, D7); //motoraansturing per motor |
alfaleader | 9:2779500685cb | 11 | Ticker pitchMotorTicker; //ticker (functie die x keer per seconde gaat uitgevoerd worden) |
alfaleader | 9:2779500685cb | 12 | void pitchMotorFunction(); //functie voor deze ticker (deze functie vullen we later in) |
BaserK | 0:40b56bdec1d2 | 13 | |
alfaleader | 9:2779500685cb | 14 | PIDControll rollMotorPID; //idem voor roll motor |
alfaleader | 9:2779500685cb | 15 | brushlessMotor rollMotor(D8, D9, D10); |
alfaleader | 9:2779500685cb | 16 | Ticker rollMotorTicker; |
alfaleader | 9:2779500685cb | 17 | void rollMotorFunction(); |
alfaleader | 9:2779500685cb | 18 | |
alfaleader | 9:2779500685cb | 19 | Ticker updatePID; // een Ticker object voor onze filter --> een ticker is een functie die x keer per seconde uitgevoerd gaat worden |
alfaleader | 9:2779500685cb | 20 | void updatePIDfunctie(); |
BaserK | 0:40b56bdec1d2 | 21 | |
alfaleader | 8:c573f315319a | 22 | //variabelen nodig voor ons programma |
BaserK | 0:40b56bdec1d2 | 23 | float pitchAngle = 0; |
alfaleader | 9:2779500685cb | 24 | float rollAngle = 0; |
BaserK | 0:40b56bdec1d2 | 25 | |
BaserK | 1:2ae94169eee6 | 26 | int main() |
BaserK | 0:40b56bdec1d2 | 27 | { |
alfaleader | 8:c573f315319a | 28 | pc.baud(9600); // PC serial connectie maken voor debugging met baud rate 9600 |
alfaleader | 8:c573f315319a | 29 | mpu6050.whoAmI(); // Communicatie test? Zijn we wel verbonden met de MPU6050 of is er een probleem met de I2C connectie |
alfaleader | 8:c573f315319a | 30 | 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 | 31 | pc.printf("Calibratie compleet \r\n"); |
alfaleader | 8:c573f315319a | 32 | mpu6050.init(); // Initializeer de gyroscoop |
alfaleader | 8:c573f315319a | 33 | pc.printf("MPU6050 is klaar \r\n\r\n"); |
BaserK | 5:477eaa33eff5 | 34 | |
alfaleader | 9:2779500685cb | 35 | updatePID.attach(&updatePIDfunctie, 0.005); // Elke 5ms onze complementary Filter aanroepen |
alfaleader | 8:c573f315319a | 36 | |
BaserK | 1:2ae94169eee6 | 37 | while(1) |
BaserK | 6:af164f09f963 | 38 | { |
alfaleader | 8:c573f315319a | 39 | pc.printf("Roll: %.1f, Pitch: %.1f\r\n",rollAngle,pitchAngle); //DEBUG |
alfaleader | 8:c573f315319a | 40 | wait_ms(500); //500ms wachten |
BaserK | 1:2ae94169eee6 | 41 | } |
BaserK | 0:40b56bdec1d2 | 42 | } |
BaserK | 0:40b56bdec1d2 | 43 | |
alfaleader | 8:c573f315319a | 44 | //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 |
alfaleader | 9:2779500685cb | 45 | void updatePIDfunctie() { |
alfaleader | 9:2779500685cb | 46 | mpu6050.complementaryFilter(&pitchAngle, &rollAngle); //lees de pitchangle en de rollangle uit vanuit de gyroscoop |
BaserK | 2:f9f4d36c2367 | 47 | |
alfaleader | 9:2779500685cb | 48 | //voer de PID regelaar uit |
alfaleader | 9:2779500685cb | 49 | pitchMotorPID.PIDaanpassing(pitchAngle); |
alfaleader | 9:2779500685cb | 50 | rollMotorPID.PIDaanpassing(rollAngle); |
BaserK | 5:477eaa33eff5 | 51 | |
alfaleader | 9:2779500685cb | 52 | //pitch aanpassingen nodig? |
alfaleader | 9:2779500685cb | 53 | if(pitchMotorPID.stop) // if the gimbal is within noise margin, dont move. |
alfaleader | 9:2779500685cb | 54 | pitchMotorTicker.detach(); |
BaserK | 5:477eaa33eff5 | 55 | else |
alfaleader | 9:2779500685cb | 56 | pitchMotorTicker.attach(&pitchMotorFunction, pitchMotorPID.delay); |
alfaleader | 9:2779500685cb | 57 | |
alfaleader | 9:2779500685cb | 58 | //roll motor aanpassingen nodig? |
alfaleader | 9:2779500685cb | 59 | if(rollMotorPID.stop) // if the gimbal is within noise margin, dont move. |
alfaleader | 9:2779500685cb | 60 | rollMotorTicker.detach(); |
alfaleader | 9:2779500685cb | 61 | else |
alfaleader | 9:2779500685cb | 62 | rollMotorTicker.attach(&rollMotorFunction, rollMotorPID.delay); |
alfaleader | 9:2779500685cb | 63 | |
BaserK | 2:f9f4d36c2367 | 64 | } |
BaserK | 5:477eaa33eff5 | 65 | |
alfaleader | 9:2779500685cb | 66 | |
alfaleader | 9:2779500685cb | 67 | void pitchMotorFunction(){ |
alfaleader | 9:2779500685cb | 68 | pitchMotor.brushlessControl(rollMotorPID.dir, 0); |
alfaleader | 9:2779500685cb | 69 | } |
alfaleader | 9:2779500685cb | 70 | |
alfaleader | 9:2779500685cb | 71 | void rollMotorFunction(){ |
alfaleader | 9:2779500685cb | 72 | rollMotor.brushlessControl(rollMotorPID.dir, 0); |
BaserK | 5:477eaa33eff5 | 73 | } |