Diff: main.cpp

Revision:

0:af4d1b736f96

diff -r 000000000000 -r af4d1b736f96 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Nov 23 03:19:28 2014 +0000
@@ -0,0 +1,88 @@
+//   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
+      }
+    }
+}