//
Dependencies: MPU6050 brushlessMotor mbed
Fork of gimbalController_brushless_IMU by
main.cpp@10:12f9371f3e0f, 2016-05-16 (annotated)
- Committer:
- alfaleader
- Date:
- Mon May 16 09:50:44 2016 +0000
- Revision:
- 10:12f9371f3e0f
- Parent:
- 9:2779500685cb
d
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"); |
alfaleader | 10:12f9371f3e0f | 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 | 10:12f9371f3e0f | 39 | pc.printf("Roll: %.1f, Pitch: %.1f\r\n",rollAngle, pitchAngle); //DEBUG |
alfaleader | 10:12f9371f3e0f | 40 | pc.printf("pitch: dir: %d, delay: %f\r\n",pitchMotorPID.dir, pitchMotorPID.delay); //DEBUG |
alfaleader | 10:12f9371f3e0f | 41 | pc.printf("roll: dir: %d, delay: %f\r\n",rollMotorPID.dir, rollMotorPID.delay); //DEBUG |
alfaleader | 8:c573f315319a | 42 | wait_ms(500); //500ms wachten |
BaserK | 1:2ae94169eee6 | 43 | } |
BaserK | 0:40b56bdec1d2 | 44 | } |
BaserK | 0:40b56bdec1d2 | 45 | |
alfaleader | 8:c573f315319a | 46 | //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 | 47 | void updatePIDfunctie() { |
alfaleader | 9:2779500685cb | 48 | mpu6050.complementaryFilter(&pitchAngle, &rollAngle); //lees de pitchangle en de rollangle uit vanuit de gyroscoop |
BaserK | 2:f9f4d36c2367 | 49 | |
alfaleader | 9:2779500685cb | 50 | //voer de PID regelaar uit |
alfaleader | 9:2779500685cb | 51 | pitchMotorPID.PIDaanpassing(pitchAngle); |
alfaleader | 9:2779500685cb | 52 | rollMotorPID.PIDaanpassing(rollAngle); |
BaserK | 5:477eaa33eff5 | 53 | |
alfaleader | 9:2779500685cb | 54 | //pitch aanpassingen nodig? |
alfaleader | 10:12f9371f3e0f | 55 | if(pitchMotorPID.stop){ // if the gimbal is within noise margin, dont move. |
alfaleader | 9:2779500685cb | 56 | pitchMotorTicker.detach(); |
alfaleader | 10:12f9371f3e0f | 57 | }else{ |
alfaleader | 9:2779500685cb | 58 | pitchMotorTicker.attach(&pitchMotorFunction, pitchMotorPID.delay); |
alfaleader | 10:12f9371f3e0f | 59 | } |
alfaleader | 9:2779500685cb | 60 | |
alfaleader | 9:2779500685cb | 61 | //roll motor aanpassingen nodig? |
alfaleader | 10:12f9371f3e0f | 62 | if(rollMotorPID.stop){ // if the gimbal is within noise margin, dont move. |
alfaleader | 9:2779500685cb | 63 | rollMotorTicker.detach(); |
alfaleader | 10:12f9371f3e0f | 64 | } else { |
alfaleader | 10:12f9371f3e0f | 65 | rollMotorTicker.attach(&rollMotorFunction, rollMotorPID.delay); |
alfaleader | 10:12f9371f3e0f | 66 | } |
alfaleader | 9:2779500685cb | 67 | |
BaserK | 2:f9f4d36c2367 | 68 | } |
BaserK | 5:477eaa33eff5 | 69 | |
alfaleader | 9:2779500685cb | 70 | |
alfaleader | 9:2779500685cb | 71 | void pitchMotorFunction(){ |
alfaleader | 10:12f9371f3e0f | 72 | pitchMotor.brushlessControl(pitchMotorPID.dir, 0); |
alfaleader | 9:2779500685cb | 73 | } |
alfaleader | 9:2779500685cb | 74 | |
alfaleader | 9:2779500685cb | 75 | void rollMotorFunction(){ |
alfaleader | 9:2779500685cb | 76 | rollMotor.brushlessControl(rollMotorPID.dir, 0); |
BaserK | 5:477eaa33eff5 | 77 | } |