智也 大野 / Mbed 2 deprecated MPU9250_edit

9軸センサー動作確認

Dependencies:   MPU9250_SPI mbed

main.cpp@0:645f2b28239f, 2017-03-17 (annotated)

Committer:
tomoya123
Date:
Fri Mar 17 08:42:47 2017 +0000
Revision:
0:645f2b28239f
9axis

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tomoya123 0:645f2b28239f 1 /*CODED by Qiyong Mu on 21/06/2014
tomoya123 0:645f2b28239f 2 kylongmu@msn.com
tomoya123 0:645f2b28239f 3 */
tomoya123 0:645f2b28239f 4 #include "mbed.h"
tomoya123 0:645f2b28239f 5 #include "MPU9250.h" //Include library
tomoya123 0:645f2b28239f 6
tomoya123 0:645f2b28239f 7 Serial pc(USBTX, USBRX);
tomoya123 0:645f2b28239f 8 Serial xbee(p9,p10);
tomoya123 0:645f2b28239f 9 DigitalOut myled(LED1);
tomoya123 0:645f2b28239f 10 //Serial pc(USBTX, USBRX);
tomoya123 0:645f2b28239f 11 SPI spi(p5, p6, p7);
tomoya123 0:645f2b28239f 12 mpu9250_spi imu(spi,p8); //define the mpu9250 object
tomoya123 0:645f2b28239f 13 int main(){
tomoya123 0:645f2b28239f 14 //float mx,my,mz,F;
tomoya123 0:645f2b28239f 15 //float ax,ay,az,acc_roll,acc_pitch,calib_az,roll_deg;
tomoya123 0:645f2b28239f 16 //float mag_x,mag_y,Orient;
tomoya123 0:645f2b28239f 17 //int i;
tomoya123 0:645f2b28239f 18 pc.baud(115200);
tomoya123 0:645f2b28239f 19 //xbee.baud(115200);
tomoya123 0:645f2b28239f 20 if(imu.init(1,BITS_DLPF_CFG_188HZ)){ //INIT the mpu9250
tomoya123 0:645f2b28239f 21 printf("\nCouldn't initialize MPU9250 via SPI!");
tomoya123 0:645f2b28239f 22 }
tomoya123 0:645f2b28239f 23 printf("\nWHOAMI=0x%2x\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104
tomoya123 0:645f2b28239f 24 wait(1);
tomoya123 0:645f2b28239f 25 printf("Gyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS)); //Set full scale range for gyros
tomoya123 0:645f2b28239f 26 wait(1);
tomoya123 0:645f2b28239f 27 printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G)); //Set full scale range for accs
tomoya123 0:645f2b28239f 28 wait(1);
tomoya123 0:645f2b28239f 29 printf("AK8963 WHIAM=0x%2x\n",imu.AK8963_whoami());
tomoya123 0:645f2b28239f 30 wait(0.1);
tomoya123 0:645f2b28239f 31 imu.AK8963_calib_Magnetometer();
tomoya123 0:645f2b28239f 32 imu.calib_acc();
tomoya123 0:645f2b28239f 33 pc.printf("good luck");
tomoya123 0:645f2b28239f 34 while(1) {
tomoya123 0:645f2b28239f 35
tomoya123 0:645f2b28239f 36 wait(0.1);
tomoya123 0:645f2b28239f 37
tomoya123 0:645f2b28239f 38 //imu.read_temp();
tomoya123 0:645f2b28239f 39 //imu.read_acc();
tomoya123 0:645f2b28239f 40 //imu.read_rot();
tomoya123 0:645f2b28239f 41 //imu.AK8963_read_Magnetometer();
tomoya123 0:645f2b28239f 42
tomoya123 0:645f2b28239f 43 imu.read_all();
tomoya123 0:645f2b28239f 44 xbee.printf("%f,%f,%f\r\n",
tomoya123 0:645f2b28239f 45 //imu.accelerometer_data[0],
tomoya123 0:645f2b28239f 46 //imu.accelerometer_data[1],
tomoya123 0:645f2b28239f 47 //imu.accelerometer_data[2],
tomoya123 0:645f2b28239f 48 imu.Magnetometer[0],
tomoya123 0:645f2b28239f 49 imu.Magnetometer[1],
tomoya123 0:645f2b28239f 50 imu.Magnetometer[2]
tomoya123 0:645f2b28239f 51 );
tomoya123 0:645f2b28239f 52 /* ax=ay=az=0;
tomoya123 0:645f2b28239f 53 for(i=0;i<100;i++){
tomoya123 0:645f2b28239f 54 ax = ax + imu.accelerometer_data[0];
tomoya123 0:645f2b28239f 55 ay = ay + imu.accelerometer_data[1];
tomoya123 0:645f2b28239f 56 az = az + imu.accelerometer_data[2];
tomoya123 0:645f2b28239f 57 }
tomoya123 0:645f2b28239f 58 wait(1.0);
tomoya123 0:645f2b28239f 59 ax=ax/100;
tomoya123 0:645f2b28239f 60 ay=ay/100;
tomoya123 0:645f2b28239f 61 az=az/100;
tomoya123 0:645f2b28239f 62 acc_roll = atan2(ay,az);
tomoya123 0:645f2b28239f 63 //roll_deg = acc_roll*180/3.1415;
tomoya123 0:645f2b28239f 64 calib_az = ay*sin(acc_roll)+az*cos(acc_roll);
tomoya123 0:645f2b28239f 65 acc_pitch = atan2(-ax,calib_az);
tomoya123 0:645f2b28239f 66 mag_x = (imu.Magnetometer[2]+13.755)*sin(acc_roll)-(imu.Magnetometer[1] - 20.469)*cos(acc_roll);
tomoya123 0:645f2b28239f 67 mag_y = (imu.Magnetometer[0] - 12.983)*cos(acc_pitch)
tomoya123 0:645f2b28239f 68 +(imu.Magnetometer[1] - 20.469)*sin(acc_pitch)*sin(acc_roll)+(imu.Magnetometer[2]+13.755)*sin(acc_pitch)*cos(acc_roll);
tomoya123 0:645f2b28239f 69 Orient = atan2(mag_x,mag_y)*180/3.1415;
tomoya123 0:645f2b28239f 70 printf("Orientation=%10.5f\n",Orient);*/
tomoya123 0:645f2b28239f 71 //printf("roll=%10.5f,pitch=%10.5f\n",roll_deg,acc_pitch);
tomoya123 0:645f2b28239f 72
tomoya123 0:645f2b28239f 73 // mx = (imu.Magnetometer[0]- 12.983)*cos(7*3.1415/180);
tomoya123 0:645f2b28239f 74 //my = (imu.Magnetometer[1]- 20.469)*cos(7*3.1415/180);
tomoya123 0:645f2b28239f 75 //mz = imu.Magnetometer[2];
tomoya123 0:645f2b28239f 76
tomoya123 0:645f2b28239f 77 // F = atan2(my,mx)*180/3.1415;
tomoya123 0:645f2b28239f 78 //printf("degree=%10.5f\n",F);
tomoya123 0:645f2b28239f 79 //myled = 0;*/
tomoya123 0:645f2b28239f 80 wait(0.5);
tomoya123 0:645f2b28239f 81 }
tomoya123 0:645f2b28239f 82 }
tomoya123 0:645f2b28239f 83