#include "mbed.h"
#include "MPU6050.h"
#include "rtos.h"

DigitalOut myled(LED1);
Serial pc1(USBTX, USBRX);
MPU6050 mpu(0x69);
int16_t ax, ay, az;
int16_t gx, gy, gz;
int moyZ=0;                 //global Z value (mean)
int16_t moy[64];            //array of different measurements


void moyennage_Z()              //calculates the mean value by going through the whole array, sum and divide by the sample size (64)
{
    for (int n=0; n<64; n++) {
        moyZ=moyZ+moy[n];
    }
    moyZ=(int)moyZ/64;
}

void mon_thr(void const *args)      //blinking thread to test mbed's state
{
    while (true) {
        myled=!myled;
        wait(0.5);
    }
}

int main()

{
    Thread thread(mon_thr);
    pc1.printf("MPU6050 test\n\n\r");               //procedure to test the connection to the mpu6050, if valid
    pc1.printf("MPU6050 initialize \n\r");          //the code to execute is embedded in a if{}
    mpu.initialize();
    pc1.printf("MPU6050 testConnection \n\r");
    bool mpu6050TestResult = mpu.testConnection();
    if(mpu6050TestResult) {
        pc1.printf("MPU6050 test passed \n\r");

        while(1) {                                             
            for (int n=0; n<64; n++) {                          //refreshing the 64-int16 array
                mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
                moy[n]=az;
                wait(0.01);
            }
            moyennage_Z();                                      //calculating the mean value by a simple sum and divide
            printf("%i\n\r",moyZ+17000);
            wait(0.1);
        }
    } else {
        pc1.printf("MPU6050 test failed \n\r");
    }
}

#include "mbed.h"
#include "MPU6050.h"
#include "rtos.h"

DigitalOut myled(LED1);
Serial pc(USBTX, USBRX);
MPU6050 mpu(0x69);
int *p;

int main()
{
    mpu.initialize();


    while(1) {

        p=(int*)malloc(sizeof(int));
        *p=(int)mpu.getAccelerationZ();
        pc.printf("%i\n\r",*p);
        free (p);
        wait(0.01);

    }

}