Diff: main.cpp

Revision:

0:ae23b58fc2d4

Child:

1:21ba826811d6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Aug 12 02:16:58 2018 +0000
@@ -0,0 +1,142 @@
+/*MPU9250_PROGRAM ver2.2 PROGRAMED BY RYOTARO FUNAI 2018/08/10*/
+#include <mbed.h>
+#include <math.h>
+
+I2C i2c(p9, p10);
+Serial pc(USBTX, USBRX);   //TWELITE使う予定なら13,14ピン
+
+#define MPU9250_ADDRESS 0x68<<1  //I2CでのMPU9250のスレーブアドレス
+#define AK8963_ADDRESS 0x0c<<1   //磁気センサのスレーブアドレス
+#define Whoami 0x75   //who_am_iレジスタのアドレス、0x71が返ってくる
+#define Magadd 0x00   //ak8963のWho_am_i
+#define PWR 0x6b      //スリープモードをonにするためのアドレス
+#define MAG_OPN 0x37  //mpu9250から磁気センサにアクセスできるようにする
+#define ACC_CONFIG 0x1c     //加速度センサ設定用のアドレス
+#define ACC_2G 0x00  //加速度センサのレンジ(2G)
+#define ACC_4G 0x08  //加速度センサのレンジ(4G)
+#define ACC_8G 0x10  //加速度センサのレンジ(8G)
+#define ACC_16G 0x18 //加速度センサのレンジ(16Gまで計測可能)
+#define MAG_CONFIG 0x0a  //磁気センサ設定用のアドレス
+#define MAG_8HZ 0x12     //磁気センサの出力周期(8Hz)
+#define MAG_100HZ 0x16   //磁気センサの出力周期(100Hz)
+#define accRange  16.0   //加速度センサの測定レンジ
+#define ST2 0x02 //磁気センサのステータスが入っているアドレス
+#define Ain 35
+#define SDA 21
+#define SCL 22
+#define led 2   //チェック用のLEDピン
+
+void Ac_Read(int16_t*, int16_t*, int16_t*);   //9軸から加速度の値を取得
+void Mag_Read(int16_t*, int16_t*, int16_t*);
+//addrにスレーブアドレス、regにアクセスするアドレスを入力する
+void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data);
+void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data);
+uint8_t IDcheck();
+
+uint8_t accgyrodata[14];
+uint8_t magneticdata[7];
+uint8_t ST2_Bit;  //磁気センサのステータスを入れておく
+
+int16_t mx, my, mz;
+double magX, magY, magZ, mag;
+float rad;
+float degree;
+float ID;
+int main() {
+    i2cWrite(MPU9250_ADDRESS, PWR, 0x00);  //スリープモードの解除
+    i2cWrite(MPU9250_ADDRESS, MAG_OPN, 0x02); //磁気センサの起動
+    i2cWrite(AK8963_ADDRESS, MAG_CONFIG, MAG_100HZ);//磁気センサの測定レンジの設定
+    double maveX = 0, maveY = 0, maveZ = 0;
+    while(1) {
+        for(int i = 0;i < 100;i++){
+            Mag_Read(&mx, &my, &mz);
+            maveX += mx;
+            maveY += my;
+            maveZ += mz;
+            wait_ms(15);
+        }          
+        maveX /= 100;
+        maveY /= 100;
+        maveZ /= 100;
+        magX = (maveX - 168.75) / 32768.0f * 4800.0f;//[uT]に変換
+        magY = (maveY - 18.75) / 32768.0f * 4800.0f;//[uT]に変換
+        pc.printf("%f,%f\n\r", magX, magY);
+        ID = IDcheck();
+        //pc.printf("%f\n\r", ID);
+        //magZ = mz / 32768.0f * 4800.0f;//[uT]に変換
+        rad = atan2(magY, magX);
+        degree = (180.0 * rad / 3.14);
+        if(degree < 0){
+            degree += 360;    
+        }
+        //pc.printf("%f\n\r",degree);
+    }
+}
+
+//mpu9250から磁気センサのみ引っ張ってくる
+void Mag_Read(int16_t* mx, int16_t* my, int16_t* mz){
+  i2cRead(AK8963_ADDRESS, ST2, 1, &ST2_Bit);//読み出し準備ができたか確認
+  if(ST2_Bit & 0x01){   //ちゃんと読めたかをST2レジスタの値を読んで確認
+    i2cRead(AK8963_ADDRESS, 0x03, 7, magneticdata);
+  }
+  else pc.printf("ERROR!!\n");
+  *mx = (magneticdata[0] << 8) | magneticdata[1];
+  *my = (magneticdata[2] << 8) | magneticdata[3];
+  *mz = (magneticdata[4] << 8) | magneticdata[5];
+}
+
+//mpu9250からデータを取得(bytesに受け取るデータのバイト数、dataに実際のデータを挿入していく)
+void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data){
+  char cmd[1];
+  char written_data[14];
+  cmd[0] = reg;
+  i2c.write(addr, cmd, 1, 1);
+  i2c.read(addr, written_data, bytes, 0);
+  for(int ii = 0; ii < bytes; ii++) {
+    data[ii] = written_data[ii];
+  }
+}
+
+//mpu9250にデータを送信(dataに送信するデータを入力する)
+void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data){
+  char cmd[2];
+  cmd[0] = reg;     //レジスタ指定
+  cmd[1] = data;    //送信するデータ
+  i2c.write(addr, cmd, 2); //レジスタ指定はどうする?
+}
+/*
+void readMagData(int16_t * destination)
+{
+  uint8_t rawData[7];  // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
+  if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
+  readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]);  // Read the six raw data and ST2 registers sequentially into data array
+  uint8_t c = rawData[6]; // End data read by reading ST2 register
+    if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
+    destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]);  // Turn the MSB and LSB into a signed 16-bit value
+    destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ;  // Data stored as little Endian
+    destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ; 
+   }
+  }
+}
+
+void getMres() {
+  switch (Mscale)
+  {
+    // Possible magnetometer scales (and their register bit settings) are:
+    // 14 bit resolution (0) and 16 bit resolution (1)
+    case MFS_14BITS:
+          mRes = 10.0*4912.0/8190.0; // Proper scale to return milliGauss
+          break;
+    case MFS_16BITS:
+          mRes = 10.0*4912.0/32760.0; // Proper scale to return milliGauss
+          break;
+  }
+}
+*/
+
+//Who_am_Iアドレスで接続確認ができる。0x48もしくは10進数で72が返ってくればok
+uint8_t IDcheck(){
+  uint8_t address;
+  i2cRead(AK8963_ADDRESS, Magadd, 1, &address);
+  return address;
+}
\ No newline at end of file