File content as of revision 2:b257a71f0655:

/*MPU9250_PROGRAM ver2.1 PROGRAMED BY RYOTARO FUNAI 2018/08/10*/
#include <mbed.h>
#include <math.h>
#define PI 3.14159265359

DigitalOut motor1(p21);
DigitalOut motor2(p22);
DigitalOut motor3(p23);
DigitalOut motor4(p24);
I2C i2c(p9, p10);
Serial pc(USBTX, USBRX);   //TWELITE使う予定なら13,14ピン
Serial gps(p28,p27);       // tx, rx
Serial twelite(p13,p14);    //twelite.printfでtwilite経由で通信できる
DigitalOut led1(p12);
AnalogIn Lx_in(p15);

//GPS用変数群
int i,rlock,mode;
char gps_data[256];
char ns,ew;
float x=135.585800, y=34.648525;    //実験用初期位地
float w_time,hokui,tokei;
float g_hokui,g_tokei,old_hokui, old_tokei; //oldとg_を比べることで距離と角度を出す
float d_hokui,m_hokui,d_tokei,m_tokei;
unsigned char c;

#define MPU9250_ADDRESS 0x68<<1  //I2CでのMPU9250のスレーブアドレス
#define AK8963_ADDRESS 0x0c<<1   //磁気センサのスレーブアドレス
#define Whoami 0x75   //who_am_iレジスタのアドレス、0x71が返ってくる
#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 LCD_Reset(); //m5stackLCDを更新
float Lx_Read();  //cdsセルから値を取得
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();
void Breky();
void Turn();
void Return();
void Right();
void Left();
void release();
void Faling();
void nikurom();
void getGPS();
void distance();
void geoDirection();
void compass();

uint8_t accgyrodata[14];
uint8_t magneticdata[7];
uint8_t ST2_Bit;  //磁気センサのステータスを入れておく

int main() { 
    gps.baud(9600);
    twelite.baud(115200);
    void Release(); //放出判定
    gps.attach(getGPS,Serial::RxIrq);   //GPS割り込み
    void Faling();//落下判定
    void nikurom(); //ニクロム線を切る
    void compass();//九軸で進む方向決める
    
}


/****************関数群****************/
//cdsセルからアナログ値を持ってくる
float Lx_Read(){
  float lx;
  lx = Lx_in.read();
  return lx;
}

//Who_am_Iアドレスで接続確認ができる。0x71もしくは10進数で113が返ってくればok
uint8_t IDcheck(){
  uint8_t address;
  i2cRead(MPU9250_ADDRESS, Whoami, 1, &address);
  return address;
}

//mpu9250から加速度センサのみ引っ張ってくる
void Ac_Read(int16_t* ax, int16_t* ay, int16_t* az){
  i2cWrite(MPU9250_ADDRESS, PWR, 0x00);  //スリープモードの解除
  i2cWrite(MPU9250_ADDRESS, ACC_CONFIG, ACC_16G);//加速度センサの測定レンジの設定
  i2cRead(MPU9250_ADDRESS, 0x3b, 14, accgyrodata);
  *ax = (accgyrodata[0] << 8) | accgyrodata[1];//accGyroTempData[0]を左に8シフトし(<<)，accGyroTempData[1]を足し合わせる(|) |は論理和
  *ay = (accgyrodata[2] << 8) | accgyrodata[3];//accGyroTempData[2]を左に8シフトし(<<)，accGyroTempData[3]を足し合わせる(|)
  *az = (accgyrodata[4] << 8) | accgyrodata[5];//accGyroTempData[4]を左に8シフトし(<<)，accGyroTempData[5]を足し合わせる(|)
}

//mpu9250から磁気センサのみ引っ張ってくる
void Mag_Read(int16_t* mx, int16_t* my, int16_t* mz){
  i2cWrite(MPU9250_ADDRESS, PWR, 0x00);  //スリープモードの解除
  i2cWrite(MPU9250_ADDRESS, MAG_OPN, 0x02); //磁気センサの起動
  i2cWrite(AK8963_ADDRESS, MAG_CONFIG, MAG_100HZ);//磁気センサの測定レンジの設定
  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 Breky(){
    motor1 = 0;
    motor2 = 0;
    motor3 = 0;
    motor4 = 0;    
}

void Turn(){
    motor1 = 1;
    motor2 = 0; 
    motor3 = 1;
    motor4 = 0;     
}

void Return(){
    motor1 = 0;
    motor2 = 1;
    motor3 = 0;
    motor4 = 1;      
}

void Left(){
    motor1 = 1;
    motor2 = 0;
    motor3 = 0;
    motor4 = 1;      
}

void Right(){
    motor1 = 0;
    motor2 = 1;
    motor3 = 1;
    motor4 = 0;      
}

void release(){
        while(1){
        //cdsセルの値の確認
        //暗いときは0.8～1.0程度の値
        float blight;
        blight = Lx_Read();
        pc.printf("%4.1f\n\r",blight);
        if(blight<=0.6) break;//   明るさが0.6以下なら次のフェーズに進む
    }
}

void Faling(){
        while(1){
        int16_t ax, ay, az;
        float accX, accY, accZ, acc;
        //加速度の値を取得し、落下判定
        Ac_Read(&ax,&ay,&az);
        accX = ax * accRange / 32768.0;//[G]に変換
        accY = ay * accRange / 32768.0;//[G]に変換
        accZ = az * accRange / 32768.0;//[G]に変換
        acc = sqrt((accX * accX) + (accY * accY) + (accZ * accZ));
        // 各軸のGを表示
        pc.printf("accX :%f\n\raccY :%f\n\raccZ :%f\n\racc :%f\n\r",accX, accY, accZ, acc);
        //静止時は1.0程度、落下時はそれを超える為、電源を入れるのは1.1以下にした
       if(acc <= 1.10){
            pc.printf("PWRON!!");
            Turn();
            break;
            //led1 = 0;
        }else{
             Breky();
            //led1 = 0;
        }
    }
}

void nikurom(){
    wait(60);
}

void getGPS() {
  c = gps.getc();
  if( c=='$' || i == 256){
    mode = 0;
    i = 0;
    for(int j=0; j<256; j++){
        gps_data[j]=NULL;
    }
  }
  
  if(mode==0){
    if((gps_data[i]=c) != '\r'){
      i++;
    }else{
      gps_data[i]='\0';
      
      if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){
        if(rlock==1){
         // pc.printf("Status:Lock(%d)\n\r",rlock);
          //logitude
          d_tokei= int(tokei/100);
          m_tokei= (tokei-d_tokei*100)/60;
          g_tokei= d_tokei+m_tokei;
          //Latitude
          d_hokui=int(hokui/100);
          m_hokui=(hokui-d_hokui*100)/60;
          g_hokui=d_hokui+m_hokui;
          //現在地のgpsデータ
          pc.printf("Lat:%.6f, Lon:%.6f\n\r",g_hokui, g_tokei);     
          twelite.printf("%.6f,%.6f\n\r",g_hokui, g_tokei);
          //前回の計測からの変化量
          //pc.printf("ch_Lat:%.6f, ch_Lon:%.6f\n\r",old_hokui-g_hokui, old_tokei-g_tokei);  
          //pc.printf("dis : %fm\n\r", 1000*6378.137*acos(sin(y*PI/180)*sin(g_hokui*PI/180)+cos(y*PI/180)*cos(g_hokui*PI/180)*cos(x*PI/180-g_tokei*PI/180)));   //別の距離の出し方
          distance();
          geoDirection();
          old_hokui=g_hokui;
          old_tokei=g_tokei;
        }else{//gpsデータ未受信時
          //pc.printf("\n\rStatus:unLock(%d)\n\r",rlock);
          ////pc.printf("%s",gps_data);
        }
      } 
    }
  }
  __disable_irq();  //割り込み禁止
}

    //ヒューベニの公式で距離出す
void distance(){
    float rlat1 = y*PI/180, rlon1 = x*PI/180;
    float rlat2 = g_hokui*PI/180, rlon2 = g_tokei*PI/180;
   float a=6378137.000, a2=pow(a,2), e=sqrt((a2-pow(6356752.314,2))/a2);
   float lat_diff = rlat1-rlat2;
   float lon_diff = rlon1-rlon2;
   float lat_ave = (rlat1+rlat2)/2;
   float w = sqrt(1-pow(e,2)*pow(sin(lat_ave),2));
   float meridian = a*(1-pow(e,2))/pow(w,3);
   float n=a/w;
   float distance2 = sqrt(pow(lat_diff*meridian,2)+pow(lon_diff*n*cos(lat_ave),2));
   //pc.printf("dis2 : %fm\n\r",distance2);
}

//角度出す
void geoDirection() {
  // 緯度 g_hokui(lat1),経度 g_tokei(lng1) の点を出発として、緯度 y(lat2),経度 x(lng2) への方位
  // 北を０度で右回りの角度０～３６０度
  float Y = cos(x*PI/180)*sin(y*PI/180-g_hokui*PI/180);
  float X = cos(g_tokei*PI/180)*sin(x*PI/180)-sin(g_tokei*PI/180)*cos(x*PI/180)*cos(y*PI/180-g_hokui*PI/180);
  float dirE0 = 180*atan2(Y, X)/PI;
  if(dirE0 < 0){
    dirE0 = dirE0 + 360;
  }
  float dirN0 = (int)(dirE0 + 90) % 360;
  pc.printf("%f\n\r",dirN0);
}

//方角だす
void compass(){
        double maveX = 0, maveY = 0, maveZ = 0;
        int16_t ax, ay, az;
        float accX, accY, accZ, acc;
        int16_t mx, my, mz;
        float magX, magY, magZ, mag;
        double rad;
        double degree;
        double roll;
        double pitch;
        //補正する為に取得
        accX = ax * accRange / 32768.0;//[G]に変換
        accY = ay * accRange / 32768.0;//[G]に変換
        accZ = az * accRange / 32768.0;//[G]に変換
        roll = atan2(accY, accZ);
        pitch = atan2(-accX, sin(accY) + cos(accZ));
        //磁気の値を取得し、方位判定
        for(int i = 0;i < 150;i++){
            Mag_Read(&mx, &my, &mz);
            maveX += mx;
            maveY += my;
            maveZ += mz;
            wait_ms(10);
        }          
        maveX /= 150;
        maveY /= 150;
        maveZ /= 150;
        magX = (maveX - 168.75) / 32768.0f * 4800.0f;//[uT]に変換
        magY = (maveY - 18.75) / 32768.0f * 4800.0f;//[uT]に変換
        magZ = mz / 32768.0f * 4800.0f;//[uT]に変換
        pc.printf("%f,%f\n\r", magX, magY);
        //ID = IDcheck();
        //pc.printf("%f\n\r", ID);
        rad = atan2(magZ * sin(roll) - magY * cos(roll), magX * cos(pitch) + magY * sin(pitch) * sin(roll) + magZ * sin(pitch) * cos(roll));
        degree = -((int)(rad * 180.0 / 3.141592 + 270.0 - 7.5) % 360 - 360.0);
        /*
        if(degree < 0){
            degree += 360;    
        }
        */
        pc.printf("%d\n\r",(int)degree);
        __enable_irq();     //割り込み許可
}