main.cpp

00001 /*MPU9250_PROGRAM ver2.2 PROGRAMED BY RYOTARO FUNAI 2018/08/10*/
00002 #include <mbed.h>
00003 #include <math.h>
00004 
00005 I2C i2c(p9, p10);
00006 Serial pc(USBTX, USBRX);   //TWELITE使う予定なら13,14ピン
00007 
00008 #define MPU9250_ADDRESS 0x68<<1  //I2CでのMPU9250のスレーブアドレス
00009 #define AK8963_ADDRESS 0x0c<<1   //磁気センサのスレーブアドレス
00010 #define Whoami 0x75   //who_am_iレジスタのアドレス、0x71が返ってくる
00011 #define Magadd 0x00   //ak8963のWho_am_i
00012 #define PWR 0x6b      //スリープモードをonにするためのアドレス
00013 #define MAG_OPN 0x37  //mpu9250から磁気センサにアクセスできるようにする
00014 #define ACC_CONFIG 0x1c     //加速度センサ設定用のアドレス
00015 #define ACC_2G 0x00  //加速度センサのレンジ(2G)
00016 #define ACC_4G 0x08  //加速度センサのレンジ(4G)
00017 #define ACC_8G 0x10  //加速度センサのレンジ(8G)
00018 #define ACC_16G 0x18 //加速度センサのレンジ(16Gまで計測可能)
00019 #define MAG_CONFIG 0x0a  //磁気センサ設定用のアドレス
00020 #define MAG_8HZ 0x12     //磁気センサの出力周期(8Hz)
00021 #define MAG_100HZ 0x16   //磁気センサの出力周期(100Hz)
00022 #define accRange  16.0   //加速度センサの測定レンジ
00023 #define ST2 0x02 //磁気センサのステータスが入っているアドレス
00024 #define Ain 35
00025 #define SDA 21
00026 #define SCL 22
00027 #define led 2   //チェック用のLEDピン
00028 
00029 void Ac_Read(int16_t*, int16_t*, int16_t*);   //9軸から加速度の値を取得
00030 void Mag_Read(int16_t*, int16_t*, int16_t*);
00031 //addrにスレーブアドレス、regにアクセスするアドレスを入力する
00032 void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data);
00033 void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data);
00034 uint8_t IDcheck();
00035 
00036 uint8_t accgyrodata[14];
00037 uint8_t magneticdata[7];
00038 uint8_t ST2_Bit;  //磁気センサのステータスを入れておく
00039 
00040 int16_t mx, my, mz;
00041 double magX, magY, magZ, mag;
00042 int16_t ax, ay, az;
00043 float accX, accY, accZ, acc;
00044 double rad;
00045 double degree;
00046 float ID;
00047 double roll;
00048 double pitch;
00049 
00050 int main() {
00051     i2cWrite(MPU9250_ADDRESS, PWR, 0x00);  //スリープモードの解除
00052     double maveX = 0, maveY = 0, maveZ = 0;
00053     while(1) {
00054         //加速度の値を取得し、落下判定
00055         Ac_Read(&ax,&ay,&az);
00056         accX = ax * accRange / 32768.0;//[G]に変換
00057         accY = ay * accRange / 32768.0;//[G]に変換
00058         accZ = az * accRange / 32768.0;//[G]に変換
00059         roll = atan2(accY, accZ);
00060         pitch = atan2(-accX, sin(accY) + cos(accZ));
00061         i2cWrite(MPU9250_ADDRESS, MAG_OPN, 0x02); //磁気センサの起動
00062         i2cWrite(AK8963_ADDRESS, MAG_CONFIG, MAG_100HZ);//磁気センサの測定レンジの設定
00063         for(int i = 0;i < 150;i++){
00064             Mag_Read(&mx, &my, &mz);
00065             maveX += mx;
00066             maveY += my;
00067             maveZ += mz;
00068             wait_ms(10);
00069         }          
00070         maveX /= 150;
00071         maveY /= 150;
00072         maveZ /= 150;
00073         magX = (maveX - 168.75) / 32768.0f * 4800.0f;//[uT]に変換
00074         magY = (maveY - 18.75) / 32768.0f * 4800.0f;//[uT]に変換
00075         magZ = mz / 32768.0f * 4800.0f;//[uT]に変換
00076         pc.printf("%f,%f\n\r", magX, magY);
00077         //ID = IDcheck();
00078         //pc.printf("%f\n\r", ID);
00079         rad = atan2(magZ * sin(roll) - magY * cos(roll), magX * cos(pitch) + magY * sin(pitch) * sin(roll) + magZ * sin(pitch) * cos(roll));
00080         degree = -((int)(rad * 180.0 / 3.141592 + 270.0 - 7.5) % 360 - 360.0);
00081         /*
00082         if(degree < 0){
00083             degree += 360;    
00084         }
00085         */
00086         pc.printf("%d\n\r",(int)degree);
00087     }
00088 }
00089 
00090 //mpu9250から加速度センサのみ引っ張ってくる
00091 void Ac_Read(int16_t* ax, int16_t* ay, int16_t* az){
00092   i2cWrite(MPU9250_ADDRESS, PWR, 0x00);  //スリープモードの解除
00093   i2cWrite(MPU9250_ADDRESS, ACC_CONFIG, ACC_16G);//加速度センサの測定レンジの設定
00094   i2cRead(MPU9250_ADDRESS, 0x3b, 14, accgyrodata);
00095   *ax = (accgyrodata[0] << 8) | accgyrodata[1];//accGyroTempData[0]を左に8シフトし(<<)，accGyroTempData[1]を足し合わせる(|) |は論理和
00096   *ay = (accgyrodata[2] << 8) | accgyrodata[3];//accGyroTempData[2]を左に8シフトし(<<)，accGyroTempData[3]を足し合わせる(|)
00097   *az = (accgyrodata[4] << 8) | accgyrodata[5];//accGyroTempData[4]を左に8シフトし(<<)，accGyroTempData[5]を足し合わせる(|)
00098 }
00099 
00100 //mpu9250から磁気センサのみ引っ張ってくる
00101 void Mag_Read(int16_t* mx, int16_t* my, int16_t* mz){
00102   i2cRead(AK8963_ADDRESS, ST2, 1, &ST2_Bit);//読み出し準備ができたか確認
00103   if(ST2_Bit & 0x01){   //ちゃんと読めたかをST2レジスタの値を読んで確認
00104     i2cRead(AK8963_ADDRESS, 0x03, 7, magneticdata);
00105   }
00106   else pc.printf("ERROR!!\n");
00107   *mx = (magneticdata[0] << 8) | magneticdata[1];
00108   *my = (magneticdata[2] << 8) | magneticdata[3];
00109   *mz = (magneticdata[4] << 8) | magneticdata[5];
00110 }
00111 
00112 //mpu9250からデータを取得(bytesに受け取るデータのバイト数、dataに実際のデータを挿入していく)
00113 void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data){
00114   char cmd[1];
00115   char written_data[14];
00116   cmd[0] = reg;
00117   i2c.write(addr, cmd, 1, 1);
00118   i2c.read(addr, written_data, bytes, 0);
00119   for(int ii = 0; ii < bytes; ii++) {
00120     data[ii] = written_data[ii];
00121   }
00122 }
00123 
00124 //mpu9250にデータを送信(dataに送信するデータを入力する)
00125 void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data){
00126   char cmd[2];
00127   cmd[0] = reg;     //レジスタ指定
00128   cmd[1] = data;    //送信するデータ
00129   i2c.write(addr, cmd, 2); //レジスタ指定はどうする?
00130 }
00131 /*
00132 void readMagData(int16_t * destination)
00133 {
00134   uint8_t rawData[7];  // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
00135   if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
00136   readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]);  // Read the six raw data and ST2 registers sequentially into data array
00137   uint8_t c = rawData[6]; // End data read by reading ST2 register
00138     if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
00139     destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]);  // Turn the MSB and LSB into a signed 16-bit value
00140     destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ;  // Data stored as little Endian
00141     destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ; 
00142    }
00143   }
00144 }
00145 
00146 void getMres() {
00147   switch (Mscale)
00148   {
00149     // Possible magnetometer scales (and their register bit settings) are:
00150     // 14 bit resolution (0) and 16 bit resolution (1)
00151     case MFS_14BITS:
00152           mRes = 10.0*4912.0/8190.0; // Proper scale to return milliGauss
00153           break;
00154     case MFS_16BITS:
00155           mRes = 10.0*4912.0/32760.0; // Proper scale to return milliGauss
00156           break;
00157   }
00158 }
00159 */
00160 
00161 //Who_am_Iアドレスで接続確認ができる。0x48もしくは10進数で72が返ってくればok
00162 uint8_t IDcheck(){
00163   uint8_t address;
00164   i2cRead(AK8963_ADDRESS, Magadd, 1, &address);
00165   return address;
00166 }