electronics-lab / Mbed 2 deprecated Cansat2018_4

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@3:42dec5cf2c03, 2018-08-10 (annotated)

Committer:
ponpoko1939
Date:
Fri Aug 10 11:08:22 2018 +0000
Revision:
3:42dec5cf2c03
Parent:
2:6745a5554c65
Child:
4:36a36b18ca30
ver2.11

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ponpoko1939 3:42dec5cf2c03 1 /*MPU9250_PROGRAM ver2.1 PROGRAMED BY RYOTARO FUNAI 2018/08/10*/
ponpoko1939 0:54e12ff8691e 2 #include <mbed.h>
ponpoko1939 0:54e12ff8691e 3 #include <math.h>
ponpoko1939 0:54e12ff8691e 4
ponpoko1939 1:70efadf7bcb0 5 DigitalOut motor1(p21);
ponpoko1939 1:70efadf7bcb0 6 DigitalOut motor2(p22);
ponpoko1939 1:70efadf7bcb0 7 DigitalOut motor3(p23);
ponpoko1939 1:70efadf7bcb0 8 DigitalOut motor4(p24);
ponpoko1939 0:54e12ff8691e 9 I2C i2c(p9, p10);
ponpoko1939 0:54e12ff8691e 10 Serial pc(USBTX, USBRX); //TWELITE使う予定なら13,14ピン
ponpoko1939 0:54e12ff8691e 11 DigitalOut led1(p12);
ponpoko1939 0:54e12ff8691e 12 AnalogIn Lx_in(p15);
ponpoko1939 0:54e12ff8691e 13
ponpoko1939 0:54e12ff8691e 14 #define MPU9250_ADDRESS 0x68<<1 //I2CでのMPU9250のスレーブアドレス
ponpoko1939 0:54e12ff8691e 15 #define AK8963_ADDRESS 0x0c<<1 //磁気センサのスレーブアドレス
ponpoko1939 0:54e12ff8691e 16 #define Whoami 0x75 //who_am_iレジスタのアドレス、0x71が返ってくる
ponpoko1939 0:54e12ff8691e 17 #define PWR 0x6b //スリープモードをonにするためのアドレス
ponpoko1939 0:54e12ff8691e 18 #define MAG_OPN 0x37 //mpu9250から磁気センサにアクセスできるようにする
ponpoko1939 0:54e12ff8691e 19 #define ACC_CONFIG 0x1c //加速度センサ設定用のアドレス
ponpoko1939 0:54e12ff8691e 20 #define ACC_2G 0x00 //加速度センサのレンジ(2G)
ponpoko1939 0:54e12ff8691e 21 #define ACC_4G 0x08 //加速度センサのレンジ(4G)
ponpoko1939 0:54e12ff8691e 22 #define ACC_8G 0x10 //加速度センサのレンジ(8G)
ponpoko1939 0:54e12ff8691e 23 #define ACC_16G 0x18 //加速度センサのレンジ(16Gまで計測可能)
ponpoko1939 0:54e12ff8691e 24 #define MAG_CONFIG 0x0a //磁気センサ設定用のアドレス
ponpoko1939 0:54e12ff8691e 25 #define MAG_8HZ 0x12 //磁気センサの出力周期(8Hz)
ponpoko1939 0:54e12ff8691e 26 #define MAG_100HZ 0x16 //磁気センサの出力周期(100Hz)
ponpoko1939 0:54e12ff8691e 27 #define accRange 16.0 //加速度センサの測定レンジ
ponpoko1939 0:54e12ff8691e 28 #define ST2 0x02 //磁気センサのステータスが入っているアドレス
ponpoko1939 0:54e12ff8691e 29 #define Ain 35
ponpoko1939 0:54e12ff8691e 30 #define SDA 21
ponpoko1939 0:54e12ff8691e 31 #define SCL 22
ponpoko1939 0:54e12ff8691e 32 #define led 2 //チェック用のLEDピン
ponpoko1939 0:54e12ff8691e 33
ponpoko1939 0:54e12ff8691e 34 //void LCD_Reset(); //m5stackLCDを更新
ponpoko1939 0:54e12ff8691e 35 float Lx_Read(); //cdsセルから値を取得
ponpoko1939 0:54e12ff8691e 36 void Ac_Read(int16_t*, int16_t*, int16_t*); //9軸から加速度の値を取得
ponpoko1939 0:54e12ff8691e 37 void Mag_Read(int16_t*, int16_t*, int16_t*);
ponpoko1939 0:54e12ff8691e 38 //addrにスレーブアドレス、regにアクセスするアドレスを入力する
ponpoko1939 0:54e12ff8691e 39 void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data);
ponpoko1939 0:54e12ff8691e 40 void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data);
ponpoko1939 0:54e12ff8691e 41 uint8_t IDcheck();
ponpoko1939 1:70efadf7bcb0 42 void Brake();
ponpoko1939 1:70efadf7bcb0 43 void Turn();
ponpoko1939 1:70efadf7bcb0 44 void Return();
ponpoko1939 1:70efadf7bcb0 45 void Right();
ponpoko1939 1:70efadf7bcb0 46 void Left();
ponpoko1939 0:54e12ff8691e 47
ponpoko1939 0:54e12ff8691e 48 uint8_t accgyrodata[14];
ponpoko1939 0:54e12ff8691e 49 uint8_t magneticdata[7];
ponpoko1939 0:54e12ff8691e 50 uint8_t ST2_Bit; //磁気センサのステータスを入れておく
ponpoko1939 0:54e12ff8691e 51
ponpoko1939 0:54e12ff8691e 52 int main() {
ponpoko1939 0:54e12ff8691e 53 while(1){
ponpoko1939 2:6745a5554c65 54 //cdsセルの値の確認
ponpoko1939 2:6745a5554c65 55 //暗いときは0.8～1.0程度の値
ponpoko1939 0:54e12ff8691e 56 float blight;
ponpoko1939 0:54e12ff8691e 57 blight = Lx_Read();
ponpoko1939 0:54e12ff8691e 58 pc.printf("%4.1f\n\r",blight);
ponpoko1939 0:54e12ff8691e 59 if(blight <= 0.6){
ponpoko1939 0:54e12ff8691e 60 int16_t ax, ay, az;
ponpoko1939 0:54e12ff8691e 61 int16_t mx, my, mz;
ponpoko1939 0:54e12ff8691e 62 float accX, accY, accZ, acc;
ponpoko1939 0:54e12ff8691e 63 float magX, magY, magZ, mag;
ponpoko1939 0:54e12ff8691e 64 int theta;
ponpoko1939 0:54e12ff8691e 65 //加速度の値を取得し、落下判定
ponpoko1939 0:54e12ff8691e 66 Ac_Read(&ax,&ay,&az);
ponpoko1939 0:54e12ff8691e 67 accX = ax * accRange / 32768.0;//[G]に変換
ponpoko1939 0:54e12ff8691e 68 accY = ay * accRange / 32768.0;//[G]に変換
ponpoko1939 0:54e12ff8691e 69 accZ = az * accRange / 32768.0;//[G]に変換
ponpoko1939 0:54e12ff8691e 70 acc = sqrt((accX * accX) + (accY * accY) + (accZ * accZ));
ponpoko1939 0:54e12ff8691e 71 //磁気の値を取得し、方位判定
ponpoko1939 0:54e12ff8691e 72 Mag_Read(&mx, &my, &mz);
ponpoko1939 0:54e12ff8691e 73 magX = (mx + 340.0f) / 32768.0f * 4800.0f;//[uT]に変換
ponpoko1939 0:54e12ff8691e 74 magY = (my - 234.0f) / 32768.0f * 4800.0f;//[uT]に変換
ponpoko1939 0:54e12ff8691e 75 magZ = mz / 32768.0f * 4800.0f;//[uT]に変換
ponpoko1939 0:54e12ff8691e 76 theta = (int)(180.0 * atan2(magY, magX) / 3.14) + 180;
ponpoko1939 0:54e12ff8691e 77 // 各軸のGを表示
ponpoko1939 0:54e12ff8691e 78 pc.printf("accX :%f\n\raccY :%f\n\raccZ :%f\n\racc :%f\n\r",accX, accY, accZ, acc);
ponpoko1939 0:54e12ff8691e 79 // 角度の表示
ponpoko1939 0:54e12ff8691e 80 pc.printf("%d\n\r", theta);
ponpoko1939 0:54e12ff8691e 81 if(acc <= 1.10){
ponpoko1939 0:54e12ff8691e 82 pc.printf("PWRON!!");
ponpoko1939 1:70efadf7bcb0 83 Turn();
ponpoko1939 0:54e12ff8691e 84 }
ponpoko1939 1:70efadf7bcb0 85 else Breky;
ponpoko1939 0:54e12ff8691e 86 }
ponpoko1939 0:54e12ff8691e 87 wait(0.5);
ponpoko1939 0:54e12ff8691e 88 }
ponpoko1939 0:54e12ff8691e 89 }
ponpoko1939 0:54e12ff8691e 90
ponpoko1939 0:54e12ff8691e 91 //cdsセルからアナログ値を持ってくる
ponpoko1939 0:54e12ff8691e 92 float Lx_Read(){
ponpoko1939 0:54e12ff8691e 93 float lx;
ponpoko1939 0:54e12ff8691e 94 lx = Lx_in.read();
ponpoko1939 0:54e12ff8691e 95 return lx;
ponpoko1939 0:54e12ff8691e 96 }
ponpoko1939 0:54e12ff8691e 97
ponpoko1939 0:54e12ff8691e 98 //Who_am_Iアドレスで接続確認ができる。0x71もしくは10進数で113が返ってくればok
ponpoko1939 0:54e12ff8691e 99 uint8_t IDcheck(){
ponpoko1939 0:54e12ff8691e 100 uint8_t address;
ponpoko1939 0:54e12ff8691e 101 i2cRead(MPU9250_ADDRESS, Whoami, 1, &address);
ponpoko1939 0:54e12ff8691e 102 return address;
ponpoko1939 0:54e12ff8691e 103 }
ponpoko1939 0:54e12ff8691e 104
ponpoko1939 0:54e12ff8691e 105 //mpu9250から加速度センサのみ引っ張ってくる
ponpoko1939 0:54e12ff8691e 106 void Ac_Read(int16_t* ax, int16_t* ay, int16_t* az){
ponpoko1939 0:54e12ff8691e 107 i2cWrite(MPU9250_ADDRESS, PWR, 0x00); //スリープモードの解除
ponpoko1939 0:54e12ff8691e 108 i2cWrite(MPU9250_ADDRESS, ACC_CONFIG, ACC_16G);//加速度センサの測定レンジの設定
ponpoko1939 0:54e12ff8691e 109 i2cRead(MPU9250_ADDRESS, 0x3b, 14, accgyrodata);
ponpoko1939 0:54e12ff8691e 110 *ax = (accgyrodata[0] << 8) | accgyrodata[1];//accGyroTempData[0]を左に8シフトし(<<)，accGyroTempData[1]を足し合わせる(|) |は論理和
ponpoko1939 0:54e12ff8691e 111 *ay = (accgyrodata[2] << 8) | accgyrodata[3];//accGyroTempData[2]を左に8シフトし(<<)，accGyroTempData[3]を足し合わせる(|)
ponpoko1939 0:54e12ff8691e 112 *az = (accgyrodata[4] << 8) | accgyrodata[5];//accGyroTempData[4]を左に8シフトし(<<)，accGyroTempData[5]を足し合わせる(|)
ponpoko1939 0:54e12ff8691e 113 }
ponpoko1939 0:54e12ff8691e 114
ponpoko1939 0:54e12ff8691e 115 //mpu9250から磁気センサのみ引っ張ってくる
ponpoko1939 0:54e12ff8691e 116 void Mag_Read(int16_t* mx, int16_t* my, int16_t* mz){
ponpoko1939 0:54e12ff8691e 117 i2cWrite(MPU9250_ADDRESS, PWR, 0x00); //スリープモードの解除
ponpoko1939 0:54e12ff8691e 118 i2cWrite(MPU9250_ADDRESS, MAG_OPN, 0x02); //磁気センサの起動
ponpoko1939 0:54e12ff8691e 119 i2cWrite(AK8963_ADDRESS, MAG_CONFIG, MAG_100HZ);//磁気センサの測定レンジの設定
ponpoko1939 0:54e12ff8691e 120 i2cRead(AK8963_ADDRESS, ST2, 1, &ST2_Bit);//読み出し準備ができたか確認
ponpoko1939 0:54e12ff8691e 121 if(ST2_Bit & 0x01){ //ちゃんと読めたかをST2レジスタの値を読んで確認
ponpoko1939 0:54e12ff8691e 122 i2cRead(AK8963_ADDRESS, 0x03, 7, magneticdata);
ponpoko1939 0:54e12ff8691e 123 }
ponpoko1939 0:54e12ff8691e 124 else pc.printf("ERROR!!\n");
ponpoko1939 0:54e12ff8691e 125 *mx = (magneticdata[0] << 8) | magneticdata[1];
ponpoko1939 0:54e12ff8691e 126 *my = (magneticdata[2] << 8) | magneticdata[3];
ponpoko1939 0:54e12ff8691e 127 *mz = (magneticdata[4] << 8) | magneticdata[5];
ponpoko1939 0:54e12ff8691e 128 }
ponpoko1939 0:54e12ff8691e 129
ponpoko1939 0:54e12ff8691e 130 //mpu9250からデータを取得(bytesに受け取るデータのバイト数、dataに実際のデータを挿入していく)
ponpoko1939 0:54e12ff8691e 131 void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data){
ponpoko1939 0:54e12ff8691e 132 char cmd[1];
ponpoko1939 0:54e12ff8691e 133 char written_data[14];
ponpoko1939 0:54e12ff8691e 134 cmd[0] = reg;
ponpoko1939 0:54e12ff8691e 135 i2c.write(addr, cmd, 1, 1);
ponpoko1939 0:54e12ff8691e 136 i2c.read(addr, written_data, bytes, 0);
ponpoko1939 0:54e12ff8691e 137 for(int ii = 0; ii < bytes; ii++) {
ponpoko1939 0:54e12ff8691e 138 data[ii] = written_data[ii];
ponpoko1939 0:54e12ff8691e 139 }
ponpoko1939 0:54e12ff8691e 140 }
ponpoko1939 0:54e12ff8691e 141
ponpoko1939 0:54e12ff8691e 142 //mpu9250にデータを送信(dataに送信するデータを入力する)
ponpoko1939 0:54e12ff8691e 143 void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data){
ponpoko1939 0:54e12ff8691e 144 char cmd[2];
ponpoko1939 0:54e12ff8691e 145 cmd[0] = reg; //レジスタ指定
ponpoko1939 0:54e12ff8691e 146 cmd[1] = data; //送信するデータ
ponpoko1939 0:54e12ff8691e 147 i2c.write(addr, cmd, 2); //レジスタ指定はどうする?
ponpoko1939 1:70efadf7bcb0 148 }
ponpoko1939 1:70efadf7bcb0 149
ponpoko1939 1:70efadf7bcb0 150 //各種モータの関数
ponpoko1939 1:70efadf7bcb0 151 void Breky(){
ponpoko1939 1:70efadf7bcb0 152 motor1 = 0;
ponpoko1939 1:70efadf7bcb0 153 motor2 = 0;
ponpoko1939 1:70efadf7bcb0 154 motor3 = 0;
ponpoko1939 1:70efadf7bcb0 155 motor4 = 0;
ponpoko1939 1:70efadf7bcb0 156 }
ponpoko1939 1:70efadf7bcb0 157
ponpoko1939 1:70efadf7bcb0 158 void Turn(){
ponpoko1939 1:70efadf7bcb0 159 motor1 = 1;
ponpoko1939 1:70efadf7bcb0 160 motor2 = 0;
ponpoko1939 1:70efadf7bcb0 161 motor3 = 1;
ponpoko1939 1:70efadf7bcb0 162 motor4 = 0;
ponpoko1939 1:70efadf7bcb0 163 }
ponpoko1939 1:70efadf7bcb0 164
ponpoko1939 1:70efadf7bcb0 165 void Return(){
ponpoko1939 1:70efadf7bcb0 166 motor1 = 0;
ponpoko1939 1:70efadf7bcb0 167 motor2 = 1;
ponpoko1939 1:70efadf7bcb0 168 motor3 = 0;
ponpoko1939 1:70efadf7bcb0 169 motor4 = 1;
ponpoko1939 1:70efadf7bcb0 170 }
ponpoko1939 1:70efadf7bcb0 171
ponpoko1939 1:70efadf7bcb0 172 void Left(){
ponpoko1939 1:70efadf7bcb0 173 motor1 = 1;
ponpoko1939 1:70efadf7bcb0 174 motor2 = 0;
ponpoko1939 1:70efadf7bcb0 175 motor3 = 0;
ponpoko1939 1:70efadf7bcb0 176 motor4 = 1;
ponpoko1939 1:70efadf7bcb0 177 }
ponpoko1939 1:70efadf7bcb0 178
ponpoko1939 1:70efadf7bcb0 179 void Right(){
ponpoko1939 1:70efadf7bcb0 180 motor1 = 0;
ponpoko1939 1:70efadf7bcb0 181 motor2 = 1;
ponpoko1939 1:70efadf7bcb0 182 motor3 = 1;
ponpoko1939 1:70efadf7bcb0 183 motor4 = 0;
ponpoko1939 0:54e12ff8691e 184 }