Yi-Wen Liao / Mbed 2 deprecated IMUtestProgram

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   IMUdriverMPU6000 mbed

main.cpp@0:af4d1b736f96, 2014-11-23 (annotated)

Committer:
ywliao
Date:
Sun Nov 23 03:19:28 2014 +0000
Revision:
0:af4d1b736f96
No history change

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ywliao 0:af4d1b736f96 1 // IMU example code
ywliao 0:af4d1b736f96 2 // 8/22/2014 edit by Grace (Yi-Wen Liao)
ywliao 0:af4d1b736f96 3
ywliao 0:af4d1b736f96 4
ywliao 0:af4d1b736f96 5 /*#include "mbed.h"
ywliao 0:af4d1b736f96 6 #include "MPU6000.h" //Include library
ywliao 0:af4d1b736f96 7 SPI spi(p11, p12, p13); //define the SPI (mosi, miso, sclk)
ywliao 0:af4d1b736f96 8 mpu6000_spi imu(spi,p22); //define the mpu6000 object
ywliao 0:af4d1b736f96 9 int main(){
ywliao 0:af4d1b736f96 10 if(imu.init(1,BITS_DLPF_CFG_5HZ)){ //INIT the mpu6000
ywliao 0:af4d1b736f96 11 printf("\nCouldn't initialize MPU6000 via SPI!");
ywliao 0:af4d1b736f96 12 }
ywliao 0:af4d1b736f96 13 wait(0.1);
ywliao 0:af4d1b736f96 14 printf("\n\nWHOAMI=%u\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104
ywliao 0:af4d1b736f96 15 wait(0.1);
ywliao 0:af4d1b736f96 16 printf("\nGyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS)); //Set full scale range for gyros
ywliao 0:af4d1b736f96 17 wait(1);
ywliao 0:af4d1b736f96 18 printf("\nAcc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G)); //Set full scale range for accs
ywliao 0:af4d1b736f96 19 wait(0.1);
ywliao 0:af4d1b736f96 20 while(1) {
ywliao 0:af4d1b736f96 21 //printf("\nT=%.3f",imu.read_temp());
ywliao 0:af4d1b736f96 22 printf(" X=%.3f\n\r",imu.read_acc(0));
ywliao 0:af4d1b736f96 23 //printf(" Y=%.3f\n\r",imu.read_acc(1));
ywliao 0:af4d1b736f96 24 //printf(" Z=%.3f\n\r",imu.read_acc(2));
ywliao 0:af4d1b736f96 25 //printf(" rX=%.3f\n\r",imu.read_rot(0));
ywliao 0:af4d1b736f96 26 //printf(" rY=%.3f\n\r",imu.read_rot(1));
ywliao 0:af4d1b736f96 27 //printf(" rZ=%.3f\n\r",imu.read_rot(2));
ywliao 0:af4d1b736f96 28 }
ywliao 0:af4d1b736f96 29 }
ywliao 0:af4d1b736f96 30 */
ywliao 0:af4d1b736f96 31
ywliao 0:af4d1b736f96 32
ywliao 0:af4d1b736f96 33
ywliao 0:af4d1b736f96 34
ywliao 0:af4d1b736f96 35
ywliao 0:af4d1b736f96 36
ywliao 0:af4d1b736f96 37
ywliao 0:af4d1b736f96 38
ywliao 0:af4d1b736f96 39
ywliao 0:af4d1b736f96 40
ywliao 0:af4d1b736f96 41
ywliao 0:af4d1b736f96 42
ywliao 0:af4d1b736f96 43
ywliao 0:af4d1b736f96 44 #include "mbed.h"
ywliao 0:af4d1b736f96 45 #include "MPU6000.h" //Include library
ywliao 0:af4d1b736f96 46 SPI spi(p11, p12, p13); //define the SPI (mosi, miso, sclk)
ywliao 0:af4d1b736f96 47 mpu6000_spi imu(spi,p22); //define the mpu6000 object
ywliao 0:af4d1b736f96 48 Timer IMUt;
ywliao 0:af4d1b736f96 49 float acc, gyro, theta;
ywliao 0:af4d1b736f96 50 float accFilterCurrent, accFilterPre, gyroFilterCurrent, gyroFliterPre;
ywliao 0:af4d1b736f96 51
ywliao 0:af4d1b736f96 52 int main(){
ywliao 0:af4d1b736f96 53
ywliao 0:af4d1b736f96 54 IMUt.start();
ywliao 0:af4d1b736f96 55 if(imu.init(1,BITS_DLPF_CFG_5HZ)){ //INIT the mpu6000
ywliao 0:af4d1b736f96 56 printf("\nCouldn't initialize MPU6000 via SPI!");
ywliao 0:af4d1b736f96 57 }
ywliao 0:af4d1b736f96 58 imu.set_gyro_scale(BITS_FS_2000DPS);
ywliao 0:af4d1b736f96 59 imu.set_acc_scale(BITS_FS_2G); //Set full scale range for accs
ywliao 0:af4d1b736f96 60 wait(1);
ywliao 0:af4d1b736f96 61
ywliao 0:af4d1b736f96 62 float acc = imu.getAccTilt();
ywliao 0:af4d1b736f96 63 float gyro = imu.read_rot(1);
ywliao 0:af4d1b736f96 64
ywliao 0:af4d1b736f96 65 float accFilterPre= 0;
ywliao 0:af4d1b736f96 66 float gyroFliterPre = 0;
ywliao 0:af4d1b736f96 67
ywliao 0:af4d1b736f96 68 while(1)
ywliao 0:af4d1b736f96 69 {
ywliao 0:af4d1b736f96 70 if (IMUt.read_ms()>=1) // read time in ms
ywliao 0:af4d1b736f96 71 {
ywliao 0:af4d1b736f96 72 accFilterCurrent = 0.8187*accFilterPre+0.1813*acc;
ywliao 0:af4d1b736f96 73 gyroFilterCurrent = 0.8187*gyroFliterPre+0.0009063*gyro;
ywliao 0:af4d1b736f96 74 theta = accFilterCurrent + gyroFilterCurrent;
ywliao 0:af4d1b736f96 75
ywliao 0:af4d1b736f96 76
ywliao 0:af4d1b736f96 77 //printf("\n\nWHOAMI=%u\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104
ywliao 0:af4d1b736f96 78 acc = imu.getAccTilt();
ywliao 0:af4d1b736f96 79 //printf("acc=%f\n\r",acc);
ywliao 0:af4d1b736f96 80 gyro = imu.read_rot(1);
ywliao 0:af4d1b736f96 81 //printf("GYRO=%f\n\r",gyro);
ywliao 0:af4d1b736f96 82 accFilterPre = accFilterCurrent;
ywliao 0:af4d1b736f96 83 gyroFliterPre = gyroFilterCurrent;
ywliao 0:af4d1b736f96 84 printf("theta=%f\n\r",theta);
ywliao 0:af4d1b736f96 85 IMUt.reset(); // reset timer
ywliao 0:af4d1b736f96 86 }
ywliao 0:af4d1b736f96 87 }
ywliao 0:af4d1b736f96 88 }