main.cpp

00001 //level 2
00002 #include "mbed.h"
00003 I2C mpu9250(p9,p10);
00004 
00005 uint8_t addrAG =  0b1101001<<1;//AD0=H(VDDIOにした)/7bitであるため. 右にズラして,左に0を追加することで, 8bitにする。　加速度ジャイロのスレーブアドレス
00006 uint8_t addrM = 0b0001100<<1;//磁気センサのスレーブアドレス
00007 double G = 9.80665;//重力加速度
00008 
00009 class mpu{//クラス宣言
00010 //メンバの宣言
00011 private:    //外部から操作不可
00012 
00013 public:     //操作可能
00014     char addr;
00015     int ac[3];//単位mV
00016     int gy[3];//単位mV
00017     int mg[3];//単位mV
00018     
00019     void ready();
00020     void who();
00021     void accel(int *ac);
00022     void gyro(int *gy);
00023     void magnet(int *mg);
00024 };
00025 
00026 void mpu::ready(){
00027     char data[7];
00028     data[0]=0x00;
00029     addr = 0x6b;
00030     mpu9250.write(addr,data,1);//0x6bに0x00を書き込む
00031     data[0]=0x02;
00032     addr = 0x37;
00033 }
00034 
00035 void mpu::who(){
00036     char data[7];
00037     int i;
00038     addr = addrAG;
00039     for(i=0;i<2;i++){
00040         data[0] = 0x75; //こいつを読んで, 0x71がかえってくる。 
00041         mpu9250.write( addr, data, 1,1);//内部レジスタを指定(1=1byte)
00042         mpu9250.read( addr,data, 1,0);//レジスタの読み取り
00043         uint8_t WHO = data[0];
00044         if(WHO == 0x71){
00045             printf("0x%x/WHO_I_AM is Yes\r\n",addr>>1);
00046             }else{
00047                 printf("0x%x/WHO_I_AM is NO\r\n",addr>>1);
00048                 }
00049         addr = addrM;
00050     }
00051 }
00052 
00053 void mpu::accel(int *ac){ //加速度//スコープ解決演算子
00054     char data[7];
00055     //int ac[3];//単位mV
00056     data[0] = 0x3b;
00057     mpu9250.write(addrAG,data,1,1);
00058     mpu9250.read(addrAG,data,7,0);
00059 
00060     ac[0]=(short)data[0]<<8|(short)data[1];
00061     ac[1]=(short)data[2]<<8|(short)data[3];
00062     ac[2]=(short)data[4]<<8|(short)data[5];
00063 }
00064 
00065 void mpu::gyro(int *gy){  //角速度//スコープ解決演算子
00066     char data[7];
00067     //int gy[3];//単位mV
00068     data[0] = 0x43;
00069     mpu9250.write(addrAG,data,1,1);
00070     mpu9250.read(addrAG,data,7,0);
00071     
00072     gy[0]=(short)data[0]<<8|(short)data[1];
00073     gy[1]=(short)data[2]<<8|(short)data[3];
00074     gy[2]=(short)data[4]<<8|(short)data[5];
00075 }
00076 
00077 void mpu::magnet(int *mg){ //地磁気//スコープ解決演算子
00078     char data[7];
00079     //int mg[3];//単位mV
00080     data[0]=0x12;
00081     addr=0x0A;
00082     mpu9250.write(addr,data,1);//AD変換(8Hz)を行う。
00083     data[0]=0x03;
00084     mpu9250.write(addrM,data,1,1);
00085     mpu9250.read(addrM,data,7,0);
00086     
00087     mg[0]=((short)data[1]<<8|(short)data[0]);
00088     mg[1]=((short)data[3]<<8|(short)data[2]);
00089     mg[2]=((short)data[5]<<8|(short)data[4]);
00090 }
00091 
00092 Serial pc(USBTX,USBRX);
00093 
00094 int main(){
00095     mpu mpu;
00096     float AX,AY,AZ;
00097     float GX,GY,GZ;
00098     float MX,MY,MZ;
00099     
00100     mpu.ready();
00101     mpu.who();
00102     
00103     int ac[3];//単位mV
00104     int gy[3];//単位mV
00105     int mg[3];//単位mV
00106     
00107     while(1){
00108         mpu.accel(ac);
00109         AX=(float)ac[0];
00110         AY=(float)ac[1];
00111         AZ=(float)ac[2];
00112         
00113         mpu.gyro(gy);        
00114         GX=(float)gy[0];
00115         GY=(float)gy[1];
00116         GZ=(float)gy[2];
00117         
00118         mpu.magnet(mg);
00119         MX=(float)mg[0];
00120         MY=(float)mg[1];
00121         MZ=(float)mg[2];
00122         
00123         printf("x%.3f   ,y%.3f  ,z%.3f  ",AX,AY,AZ);
00124         printf("x%.3f   ,y%.3f  ,z%.3f  ",GX,GY,GZ);
00125         printf("x%.3f   ,y%.3f  ,z%.3f\r\n",MX,MY,MZ);
00126         
00127         wait(0.01);
00128     }
00129     
00130 }