File content as of revision 1:2ae94169eee6:

#include "mbed.h"
#include "MPU6050.h"
#include "ledControl.h"
#include "brushlessController_TB6612FNG.h"

Serial pc(USBTX, USBRX);       // Create terminal link
MPU6050 mpu6050;               // mpu6050 object from MPU6050 classs
Ticker toggler1;               // Ticker for led toggling
Ticker filter;                 // Ticker for periodic call to compFilter funcçs 

/* Function prototypes */
void toggle_led1();
void toggle_led2();
void compFilter();

float pitchAngle = 0;
float rollAngle = 0;
int errorMargin = 8;   // error margin in degrees for stabilizing the gimbal system
int setPoint = -30;      // set point in degrees for the gimbal system 

int main()
{
    pc.baud(9600);                               // baud rate: 9600
    i2c.frequency(400000);                       // fast i2c: 400 kHz
    mpu6050.whoAmI();                            // Communication test: WHO_AM_I register reading 
    mpu6050.calibrate(accelBias,gyroBias);       // Calibrate MPU6050 and load biases into bias registers
    pc.printf("Calibration is completed. \r\n"); 
    mpu6050.init();                              // Initialize the sensor
    pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
    
    filter.attach(&compFilter, 0.005);           // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
    while(1)
    {
        if (abs(rollAngle - setPoint) < errorMargin)
        {
            // Do not move if above statement is true
            led4 = 1;
        }          
        else if(rollAngle > setPoint)
        {
            brushlessControl(1, 500);  
            led4 = 0;
        }
        else 
        {   
            brushlessControl(0, 500);  
            led4 = 0;
        }
    }    
}

void toggle_led1() {ledToggle(1);}
void toggle_led2() {ledToggle(2);}

/* This function is created to avoid address error that caused from Ticker.attach func */ 
void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}