Jannes De Brabandere / Mbed 2 deprecated gimbalController_brushless_IMU

//

Dependencies:   MPU6050 brushlessMotor mbed

Fork of gimbalController_brushless_IMU by Baser Kandehir

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?

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");
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 }