File content as of revision 10:12f9371f3e0f:

#include "mbed.h"
#include "MPU6050.h"
#include "PID.h"
#include "brushlessMotor.h"

Serial pc(USBTX, USBRX);       // Debug serial connectie (voor de computer)
MPU6050 mpu6050;               // mpu6050 object voor onze gyroscoop

PIDControll pitchMotorPID; //PID controle aanmaken voor de pitchmotor
brushlessMotor pitchMotor(D5, D6, D7); //motoraansturing per motor
Ticker pitchMotorTicker; //ticker (functie die x keer per seconde gaat uitgevoerd worden)
void pitchMotorFunction(); //functie voor deze ticker (deze functie vullen we later in)

PIDControll rollMotorPID; //idem voor roll motor
brushlessMotor rollMotor(D8, D9, D10);
Ticker rollMotorTicker;
void rollMotorFunction();

Ticker updatePID;                 // een Ticker object voor onze filter --> een ticker is een functie die x keer per seconde uitgevoerd gaat worden
void updatePIDfunctie();

//variabelen nodig voor ons programma
float pitchAngle = 0;
float rollAngle = 0;         

int main()
{
    pc.baud(9600);                               // PC serial connectie maken voor debugging met baud rate 9600
    mpu6050.whoAmI();                            // Communicatie test? Zijn we wel verbonden met de MPU6050 of is er een probleem met de I2C connectie
    mpu6050.calibrate(accelBias,gyroBias);       // Calibreren en upload deze waarden in het MPU6050 bias register, dit is onze 0 waarde waar de camera moet izjn
    pc.printf("Calibratie compleet \r\n"); 
    mpu6050.init();                              // Initializeer de gyroscoop
    pc.printf("MPU6050 is klaar \r\n\r\n");

    updatePID.attach(&updatePIDfunctie, 0.005);       // Elke 5ms onze complementary Filter aanroepen
    
    while(1)
    {          
       pc.printf("Roll: %.1f, Pitch: %.1f\r\n",rollAngle, pitchAngle); //DEBUG
       pc.printf("pitch: dir: %d, delay: %f\r\n",pitchMotorPID.dir, pitchMotorPID.delay); //DEBUG
       pc.printf("roll: dir: %d, delay: %f\r\n",rollMotorPID.dir, rollMotorPID.delay); //DEBUG
       wait_ms(500); //500ms wachten
    }    
}

//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
void updatePIDfunctie() {
    mpu6050.complementaryFilter(&pitchAngle, &rollAngle); //lees de pitchangle en de rollangle uit vanuit de gyroscoop
    
    //voer de PID regelaar uit
    pitchMotorPID.PIDaanpassing(pitchAngle);
    rollMotorPID.PIDaanpassing(rollAngle);
    
    //pitch aanpassingen nodig?
    if(pitchMotorPID.stop){  // if the gimbal is within noise margin, dont move.
        pitchMotorTicker.detach();
    }else{
        pitchMotorTicker.attach(&pitchMotorFunction, pitchMotorPID.delay);  
    }
    
    //roll motor aanpassingen nodig?
    if(rollMotorPID.stop){  // if the gimbal is within noise margin, dont move.
        rollMotorTicker.detach();
    } else { 
        rollMotorTicker.attach(&rollMotorFunction, rollMotorPID.delay); 
    } 

}


void pitchMotorFunction(){
    pitchMotor.brushlessControl(pitchMotorPID.dir, 0);    
}

void rollMotorFunction(){
     rollMotor.brushlessControl(rollMotorPID.dir, 0);    
}