File content as of revision 0:af4d1b736f96:

//   IMU example code
//   8/22/2014 edit by Grace (Yi-Wen Liao)


/*#include "mbed.h"
#include "MPU6000.h"        //Include library
SPI spi(p11, p12, p13);     //define the SPI (mosi, miso, sclk)
mpu6000_spi imu(spi,p22);   //define the mpu6000 object
int main(){
    if(imu.init(1,BITS_DLPF_CFG_5HZ)){  //INIT the mpu6000
        printf("\nCouldn't initialize MPU6000 via SPI!");
    }    
    wait(0.1);    
    printf("\n\nWHOAMI=%u\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104
    wait(0.1);    
    printf("\nGyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS));    //Set full scale range for gyros
    wait(1);  
    printf("\nAcc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G));          //Set full scale range for accs
    wait(0.1);    
    while(1) {
        //printf("\nT=%.3f",imu.read_temp());
        printf(" X=%.3f\n\r",imu.read_acc(0));  
        //printf(" Y=%.3f\n\r",imu.read_acc(1)); 
        //printf(" Z=%.3f\n\r",imu.read_acc(2));   
        //printf(" rX=%.3f\n\r",imu.read_rot(0));  
        //printf(" rY=%.3f\n\r",imu.read_rot(1)); 
        //printf(" rZ=%.3f\n\r",imu.read_rot(2));
    }
}
*/













#include "mbed.h"
#include "MPU6000.h"        //Include library
SPI spi(p11, p12, p13);     //define the SPI (mosi, miso, sclk)
mpu6000_spi imu(spi,p22);   //define the mpu6000 object
Timer IMUt;
float acc, gyro, theta;
float accFilterCurrent, accFilterPre, gyroFilterCurrent, gyroFliterPre; 

int main(){
    
    IMUt.start();
    if(imu.init(1,BITS_DLPF_CFG_5HZ)){  //INIT the mpu6000
    printf("\nCouldn't initialize MPU6000 via SPI!");
    } 
    imu.set_gyro_scale(BITS_FS_2000DPS); 
    imu.set_acc_scale(BITS_FS_2G);          //Set full scale range for accs
    wait(1);
    
    float acc = imu.getAccTilt();
    float gyro = imu.read_rot(1);
    
    float accFilterPre= 0;
    float gyroFliterPre = 0;
    
    while(1) 
    { 
      if (IMUt.read_ms()>=1) // read time in ms
      { 
          accFilterCurrent = 0.8187*accFilterPre+0.1813*acc;
          gyroFilterCurrent = 0.8187*gyroFliterPre+0.0009063*gyro;
          theta = accFilterCurrent + gyroFilterCurrent;
          
              
          //printf("\n\nWHOAMI=%u\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104
          acc = imu.getAccTilt();
          //printf("acc=%f\n\r",acc);
          gyro = imu.read_rot(1);
          //printf("GYRO=%f\n\r",gyro);
          accFilterPre = accFilterCurrent;
          gyroFliterPre = gyroFilterCurrent;
          printf("theta=%f\n\r",theta);
          IMUt.reset(); // reset timer
      }
    }
}