File content as of revision 3:42dec5cf2c03:

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

DigitalOut motor1(p21);
DigitalOut motor2(p22);
DigitalOut motor3(p23);
DigitalOut motor4(p24);
I2C i2c(p9, p10);
Serial pc(USBTX, USBRX);   //TWELITE使う予定なら13,14ピン
DigitalOut led1(p12);
AnalogIn Lx_in(p15);

#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 Brake();
void Turn();
void Return();
void Right();
void Left();

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

int main() {
    while(1){
        //cdsセルの値の確認
        //暗いときは0.8～1.0程度の値
        float blight;
        blight = Lx_Read();
        pc.printf("%4.1f\n\r",blight);
        if(blight <= 0.6){
            int16_t ax, ay, az;
            int16_t mx, my, mz;
            float accX, accY, accZ, acc;
            float magX, magY, magZ, mag;
            int theta;
            //加速度の値を取得し、落下判定
            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));
            //磁気の値を取得し、方位判定
            Mag_Read(&mx, &my, &mz);
            magX = (mx + 340.0f) / 32768.0f * 4800.0f;//[uT]に変換
            magY = (my - 234.0f) / 32768.0f * 4800.0f;//[uT]に変換
            magZ = mz / 32768.0f * 4800.0f;//[uT]に変換
            theta = (int)(180.0 * atan2(magY, magX) / 3.14) + 180;
            // 各軸のGを表示
            pc.printf("accX :%f\n\raccY :%f\n\raccZ :%f\n\racc :%f\n\r",accX, accY, accZ, acc);
            // 角度の表示
            pc.printf("%d\n\r", theta);
            if(acc <= 1.10){
                pc.printf("PWRON!!");
                Turn();
            }
            else Breky;
        }
        wait(0.5);
    }
}

//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;      
}