File content as of revision 0:df9740ffcf1d:

#include "mbed.h"
#include <MPU6050.h>
#include "millis.h"
#include "PID.h"
#include "motor.h"
#define pi 3.14159265358979323846

Serial pc(USBTX,USBRX);

Serial bluetooth(D10, D2);//Rx, Tx (according to the bt module)
InterruptIn receiver(PC_13);//interupt for bt

I2C i2c(D14, D15);//SDA, SCL

PwmOut pwm1(D13), pwm2(D12);//motor PWM for motor1 and 2 respectively 
DigitalOut dir1(D7), dir2(D6), dir3(D5), dir4(D4);//motor inputs (dir1 and 2 for motor 1, dir and 4 for motor 2)

MPU6050 mpu;//mpu library
PID pid;//PID lib
MOTOR m;//motor lib

float times, n_time, o_time, offset, sp, r, x, integral, out[3], gyro_val[3], acc_val[3], data[3], old_p;//init floats
int counter = 0, flag = 0;//init counter
char oneo[2];//char that stores bt command

void bt()
{
    oneo[0] = bluetooth.getc();//read char sent to bt module
    flag = 1;//set interrupt flag to 1 
}

int main() {
    mpu.init();//initialize the mpu
    mpu.whoAmI();//check mpu connection
    pwm1.period(0.001);//period for motor 1
    pwm2.period(0.001);//period for motor 2
    bluetooth.baud(9600);//bt baud rate (same as pc)
    pc.baud(9600);
    bluetooth.attach(&bt);//attach the interrupt 
    for(int c = 0; c<10; c++)//get the initial position of the mpu
    {
        mpu.gyro(gyro_val);//read gyro
        mpu.acc(acc_val);//read accelerometer
        offset += gyro_val[0];//add gyro value to "offset" to find gyro offset
        r = sqrt(acc_val[0]*acc_val[0]+acc_val[1]*acc_val[1]+acc_val[2]*acc_val[2]);//resultan force from all accelerometer axis
        x = (acos(acc_val[0]/r))*180/pi;//find the roll on x axis
        sp += x;//find the starting position of the accelerometer
        if(gyro_val[0] == 0)//test in case the mpu haven't finished initializing
        {c--;}
        //pc.printf("test");
    } 
    offset = offset/10;//average the offset and starting position
    sp = sp/10;
    //pc.printf("%f \n\r", sp);
    millisStart();//start counter that counts every millisecond 
    while(1)
    {
     if (flag == 0)//if no intterrupt
          {
            n_time = millis();//get the time
            times = (n_time - o_time)*0.001;//how many second(s) since the code is run
            mpu.filtered(old_p, data, times, offset, sp);//get filtered data
            old_p = data[0];//store old data (for gyro)
            pid.control(data[0], out, times, integral);//use the pid
            m.balance(out[0]);//put the pid output as motor speed (-1 to 1 scale)
            o_time = n_time;//store the time
          }
          else if(flag == 1)//if interrupt flag is triggered
         {
             if(oneo[0] == 'w')//if w char is sent
             {m.forward();}//go forward
             else if(oneo[0] == 's')
             {m.backward();}
             else if(oneo[0] == 'a')
             {m.left();}
             else if(oneo[0] == 'd')
             {m.right();}             
             flag = 0;//set intterupt flag to 0
             wait(0.3);//let motor run for 0.3 sec
         }
    }
}