Jannes De Brabandere / Mbed 2 deprecated gimbalController_brushless_IMU

//

Dependencies:   MPU6050 brushlessMotor mbed

Fork of gimbalController_brushless_IMU by Baser Kandehir

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?

UserRevisionLine numberNew 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 }