File content as of revision 8:c573f315319a:

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

Serial pc(USBTX, USBRX);       // Debug serial connectie (voor de computer)
MPU6050 mpu6050;               // mpu6050 object voor onze gyroscoop
Ticker filter;                 // een Ticker object voor onze filter --> een ticker is een functie die x keer per seconde uitgevoerd gaat worden
Ticker gimbal;                 // ticker voor de gimball
Ticker speed;                  // ticker voor de snelheids controle

//Functie prototypes, zodat het programma weet dat we deze functies later gaan "invullen"
void compFilter();
void gimbalPID();
void speedControl();

//variabelen nodig voor ons programma
float pitchAngle = 0;
float rollAngle = 0;
bool dir;           // links of rechts
bool stop;          // motor moet stoppen?
float delay;        // delay tussen onze stappen

//PID
float Kp = 10;
float Ki = 0.0001; 
float Kd = 5;
float set_point = 0;         // camera hoek
float proportional = 0;
float last_proportional =0;
float integral = 0;
float derivative = 0;
float errorPID = 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");
   
    filter.attach(&compFilter, 0.005);       // Elke 5ms onze complementary Filter aanroepen
    gimbal.attach(&gimbalPID,  0.005);       // Gimbal aansturing ook elke 5ms aanroepen
    
    while(1)
    {          
       pc.printf("Roll: %.1f, Pitch: %.1f\r\n",rollAngle,pitchAngle); //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 compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}

//elke 5ms updaten we ook de gimball
void gimbalPID()
{
    proportional = set_point - rollAngle;        
    integral += proportional; 
    derivative = proportional - last_proportional;
    last_proportional = proportional;
    
    errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative); 
    (errorPID > 0)?(dir = 1):(dir = 0);
    
    // errorPID is restricted between -400 and 400 
    if(errorPID > 400)
        errorPID = 400;
    else if(errorPID < -400)
        errorPID = -400;   
   
    stop = 0;   
    delay = 0.1/abs(errorPID);      // speed should be proportional to error, therefore time delay
                                     //between steps should be inverse proportional to error.
                                      
    if (abs(errorPID)< Kp/2) stop = 1;  // 0.5 deg noise margin
         
    if(stop)  // if the gimbal is within noise margin, dont move.
        speed.detach();
    else    
        speed.attach(&speedControl, delay);  
}

void speedControl() 
{    
    brushlessControl(dir, 0);    
}