main.cpp

00001 /*MPU9250_PROGRAM ver2.1 PROGRAMED BY RYOTARO FUNAI 2018/08/10*/
00002 #include <mbed.h>
00003 #include <math.h>
00004 #define PI 3.14159265359
00005 
00006 DigitalOut motor1(p21);
00007 DigitalOut motor2(p22);
00008 DigitalOut motor3(p23);
00009 DigitalOut motor4(p24);
00010 I2C i2c(p9, p10);
00011 Serial pc(USBTX, USBRX);   //TWELITE使う予定なら13,14ピン
00012 Serial gps(p28,p27);       // tx, rx
00013 Serial twelite(p13,p14);    //twelite.printfでtwilite経由で通信できる
00014 DigitalOut led1(p12);
00015 AnalogIn Lx_in(p15);
00016 
00017 //GPS用変数群
00018 int i,rlock,mode;
00019 char gps_data[256];
00020 char ns,ew;
00021 float x=135.585800, y=34.648525;    //実験用初期位地
00022 float w_time,hokui,tokei;
00023 float g_hokui,g_tokei,old_hokui, old_tokei; //oldとg_を比べることで距離と角度を出す
00024 float d_hokui,m_hokui,d_tokei,m_tokei;
00025 unsigned char c;
00026 
00027 #define MPU9250_ADDRESS 0x68<<1  //I2CでのMPU9250のスレーブアドレス
00028 #define AK8963_ADDRESS 0x0c<<1   //磁気センサのスレーブアドレス
00029 #define Whoami 0x75   //who_am_iレジスタのアドレス、0x71が返ってくる
00030 #define PWR 0x6b      //スリープモードをonにするためのアドレス
00031 #define MAG_OPN 0x37  //mpu9250から磁気センサにアクセスできるようにする
00032 #define ACC_CONFIG 0x1c     //加速度センサ設定用のアドレス
00033 #define ACC_2G 0x00  //加速度センサのレンジ(2G)
00034 #define ACC_4G 0x08  //加速度センサのレンジ(4G)
00035 #define ACC_8G 0x10  //加速度センサのレンジ(8G)
00036 #define ACC_16G 0x18 //加速度センサのレンジ(16Gまで計測可能)
00037 #define MAG_CONFIG 0x0a  //磁気センサ設定用のアドレス
00038 #define MAG_8HZ 0x12     //磁気センサの出力周期(8Hz)
00039 #define MAG_100HZ 0x16   //磁気センサの出力周期(100Hz)
00040 #define accRange  16.0   //加速度センサの測定レンジ
00041 #define ST2 0x02 //磁気センサのステータスが入っているアドレス
00042 #define Ain 35
00043 #define SDA 21
00044 #define SCL 22
00045 #define led 2   //チェック用のLEDピン
00046 
00047 //void LCD_Reset(); //m5stackLCDを更新
00048 float Lx_Read();  //cdsセルから値を取得
00049 void Ac_Read(int16_t*, int16_t*, int16_t*);   //9軸から加速度の値を取得
00050 void Mag_Read(int16_t*, int16_t*, int16_t*);
00051 //addrにスレーブアドレス、regにアクセスするアドレスを入力する
00052 void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data);
00053 void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data);
00054 uint8_t IDcheck();
00055 void Breky();
00056 void Turn();
00057 void Return();
00058 void Right();
00059 void Left();
00060 void release();
00061 void Faling();
00062 void nikurom();
00063 void getGPS();
00064 void distance();
00065 void geoDirection();
00066 void compass();
00067 
00068 uint8_t accgyrodata[14];
00069 uint8_t magneticdata[7];
00070 uint8_t ST2_Bit;  //磁気センサのステータスを入れておく
00071 
00072 int main() { 
00073     gps.baud(9600);
00074     twelite.baud(115200);
00075     void Release(); //放出判定
00076     gps.attach(getGPS,Serial::RxIrq);   //GPS割り込み
00077     void Faling();//落下判定
00078     void nikurom(); //ニクロム線を切る
00079     void compass();//九軸で進む方向決める
00080     
00081 }
00082 
00083 
00084 /****************関数群****************/
00085 //cdsセルからアナログ値を持ってくる
00086 float Lx_Read(){
00087   float lx;
00088   lx = Lx_in.read();
00089   return lx;
00090 }
00091 
00092 //Who_am_Iアドレスで接続確認ができる。0x71もしくは10進数で113が返ってくればok
00093 uint8_t IDcheck(){
00094   uint8_t address;
00095   i2cRead(MPU9250_ADDRESS, Whoami, 1, &address);
00096   return address;
00097 }
00098 
00099 //mpu9250から加速度センサのみ引っ張ってくる
00100 void Ac_Read(int16_t* ax, int16_t* ay, int16_t* az){
00101   i2cWrite(MPU9250_ADDRESS, PWR, 0x00);  //スリープモードの解除
00102   i2cWrite(MPU9250_ADDRESS, ACC_CONFIG, ACC_16G);//加速度センサの測定レンジの設定
00103   i2cRead(MPU9250_ADDRESS, 0x3b, 14, accgyrodata);
00104   *ax = (accgyrodata[0] << 8) | accgyrodata[1];//accGyroTempData[0]を左に8シフトし(<<)，accGyroTempData[1]を足し合わせる(|) |は論理和
00105   *ay = (accgyrodata[2] << 8) | accgyrodata[3];//accGyroTempData[2]を左に8シフトし(<<)，accGyroTempData[3]を足し合わせる(|)
00106   *az = (accgyrodata[4] << 8) | accgyrodata[5];//accGyroTempData[4]を左に8シフトし(<<)，accGyroTempData[5]を足し合わせる(|)
00107 }
00108 
00109 //mpu9250から磁気センサのみ引っ張ってくる
00110 void Mag_Read(int16_t* mx, int16_t* my, int16_t* mz){
00111   i2cWrite(MPU9250_ADDRESS, PWR, 0x00);  //スリープモードの解除
00112   i2cWrite(MPU9250_ADDRESS, MAG_OPN, 0x02); //磁気センサの起動
00113   i2cWrite(AK8963_ADDRESS, MAG_CONFIG, MAG_100HZ);//磁気センサの測定レンジの設定
00114   i2cRead(AK8963_ADDRESS, ST2, 1, &ST2_Bit);//読み出し準備ができたか確認
00115   if(ST2_Bit & 0x01){   //ちゃんと読めたかをST2レジスタの値を読んで確認
00116     i2cRead(AK8963_ADDRESS, 0x03, 7, magneticdata);
00117   }
00118   else pc.printf("ERROR!!\n");
00119   *mx = (magneticdata[0] << 8) | magneticdata[1];
00120   *my = (magneticdata[2] << 8) | magneticdata[3];
00121   *mz = (magneticdata[4] << 8) | magneticdata[5];
00122 }
00123 
00124 //mpu9250からデータを取得(bytesに受け取るデータのバイト数、dataに実際のデータを挿入していく)
00125 void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data){
00126   char cmd[1];
00127   char written_data[14];
00128   cmd[0] = reg;
00129   i2c.write(addr, cmd, 1, 1);
00130   i2c.read(addr, written_data, bytes, 0);
00131   for(int ii = 0; ii < bytes; ii++) {
00132     data[ii] = written_data[ii];
00133   }
00134 }
00135 
00136 //mpu9250にデータを送信(dataに送信するデータを入力する)
00137 void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data){
00138   char cmd[2];
00139   cmd[0] = reg;     //レジスタ指定
00140   cmd[1] = data;    //送信するデータ
00141   i2c.write(addr, cmd, 2); //レジスタ指定はどうする?
00142 }
00143 
00144 //各種モータの関数
00145 void Breky(){
00146     motor1 = 0;
00147     motor2 = 0;
00148     motor3 = 0;
00149     motor4 = 0;    
00150 }
00151 
00152 void Turn(){
00153     motor1 = 1;
00154     motor2 = 0; 
00155     motor3 = 1;
00156     motor4 = 0;     
00157 }
00158 
00159 void Return(){
00160     motor1 = 0;
00161     motor2 = 1;
00162     motor3 = 0;
00163     motor4 = 1;      
00164 }
00165 
00166 void Left(){
00167     motor1 = 1;
00168     motor2 = 0;
00169     motor3 = 0;
00170     motor4 = 1;      
00171 }
00172 
00173 void Right(){
00174     motor1 = 0;
00175     motor2 = 1;
00176     motor3 = 1;
00177     motor4 = 0;      
00178 }
00179 
00180 void release(){
00181         while(1){
00182         //cdsセルの値の確認
00183         //暗いときは0.8～1.0程度の値
00184         float blight;
00185         blight = Lx_Read();
00186         pc.printf("%4.1f\n\r",blight);
00187         if(blight<=0.6) break;//   明るさが0.6以下なら次のフェーズに進む
00188     }
00189 }
00190 
00191 void Faling(){
00192         while(1){
00193         int16_t ax, ay, az;
00194         float accX, accY, accZ, acc;
00195         //加速度の値を取得し、落下判定
00196         Ac_Read(&ax,&ay,&az);
00197         accX = ax * accRange / 32768.0;//[G]に変換
00198         accY = ay * accRange / 32768.0;//[G]に変換
00199         accZ = az * accRange / 32768.0;//[G]に変換
00200         acc = sqrt((accX * accX) + (accY * accY) + (accZ * accZ));
00201         // 各軸のGを表示
00202         pc.printf("accX :%f\n\raccY :%f\n\raccZ :%f\n\racc :%f\n\r",accX, accY, accZ, acc);
00203         //静止時は1.0程度、落下時はそれを超える為、電源を入れるのは1.1以下にした
00204        if(acc <= 1.10){
00205             pc.printf("PWRON!!");
00206             Turn();
00207             break;
00208             //led1 = 0;
00209         }else{
00210              Breky();
00211             //led1 = 0;
00212         }
00213     }
00214 }
00215 
00216 void nikurom(){
00217     wait(60);
00218 }
00219 
00220 void getGPS() {
00221   c = gps.getc();
00222   if( c=='$' || i == 256){
00223     mode = 0;
00224     i = 0;
00225     for(int j=0; j<256; j++){
00226         gps_data[j]=NULL;
00227     }
00228   }
00229   
00230   if(mode==0){
00231     if((gps_data[i]=c) != '\r'){
00232       i++;
00233     }else{
00234       gps_data[i]='\0';
00235       
00236       if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){
00237         if(rlock==1){
00238          // pc.printf("Status:Lock(%d)\n\r",rlock);
00239           //logitude
00240           d_tokei= int(tokei/100);
00241           m_tokei= (tokei-d_tokei*100)/60;
00242           g_tokei= d_tokei+m_tokei;
00243           //Latitude
00244           d_hokui=int(hokui/100);
00245           m_hokui=(hokui-d_hokui*100)/60;
00246           g_hokui=d_hokui+m_hokui;
00247           //現在地のgpsデータ
00248           pc.printf("Lat:%.6f, Lon:%.6f\n\r",g_hokui, g_tokei);     
00249           twelite.printf("%.6f,%.6f\n\r",g_hokui, g_tokei);
00250           //前回の計測からの変化量
00251           //pc.printf("ch_Lat:%.6f, ch_Lon:%.6f\n\r",old_hokui-g_hokui, old_tokei-g_tokei);  
00252           //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)));   //別の距離の出し方
00253           distance();
00254           geoDirection();
00255           old_hokui=g_hokui;
00256           old_tokei=g_tokei;
00257         }else{//gpsデータ未受信時
00258           //pc.printf("\n\rStatus:unLock(%d)\n\r",rlock);
00259           ////pc.printf("%s",gps_data);
00260         }
00261       } 
00262     }
00263   }
00264   __disable_irq();  //割り込み禁止
00265 }
00266 
00267     //ヒューベニの公式で距離出す
00268 void distance(){
00269     float rlat1 = y*PI/180, rlon1 = x*PI/180;
00270     float rlat2 = g_hokui*PI/180, rlon2 = g_tokei*PI/180;
00271    float a=6378137.000, a2=pow(a,2), e=sqrt((a2-pow(6356752.314,2))/a2);
00272    float lat_diff = rlat1-rlat2;
00273    float lon_diff = rlon1-rlon2;
00274    float lat_ave = (rlat1+rlat2)/2;
00275    float w = sqrt(1-pow(e,2)*pow(sin(lat_ave),2));
00276    float meridian = a*(1-pow(e,2))/pow(w,3);
00277    float n=a/w;
00278    float distance2 = sqrt(pow(lat_diff*meridian,2)+pow(lon_diff*n*cos(lat_ave),2));
00279    //pc.printf("dis2 : %fm\n\r",distance2);
00280 }
00281 
00282 //角度出す
00283 void geoDirection() {
00284   // 緯度 g_hokui(lat1),経度 g_tokei(lng1) の点を出発として、緯度 y(lat2),経度 x(lng2) への方位
00285   // 北を０度で右回りの角度０～３６０度
00286   float Y = cos(x*PI/180)*sin(y*PI/180-g_hokui*PI/180);
00287   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);
00288   float dirE0 = 180*atan2(Y, X)/PI;
00289   if(dirE0 < 0){
00290     dirE0 = dirE0 + 360;
00291   }
00292   float dirN0 = (int)(dirE0 + 90) % 360;
00293   pc.printf("%f\n\r",dirN0);
00294 }
00295 
00296 //方角だす
00297 void compass(){
00298         double maveX = 0, maveY = 0, maveZ = 0;
00299         int16_t ax, ay, az;
00300         float accX, accY, accZ, acc;
00301         int16_t mx, my, mz;
00302         float magX, magY, magZ, mag;
00303         double rad;
00304         double degree;
00305         double roll;
00306         double pitch;
00307         //補正する為に取得
00308         accX = ax * accRange / 32768.0;//[G]に変換
00309         accY = ay * accRange / 32768.0;//[G]に変換
00310         accZ = az * accRange / 32768.0;//[G]に変換
00311         roll = atan2(accY, accZ);
00312         pitch = atan2(-accX, sin(accY) + cos(accZ));
00313         //磁気の値を取得し、方位判定
00314         for(int i = 0;i < 150;i++){
00315             Mag_Read(&mx, &my, &mz);
00316             maveX += mx;
00317             maveY += my;
00318             maveZ += mz;
00319             wait_ms(10);
00320         }          
00321         maveX /= 150;
00322         maveY /= 150;
00323         maveZ /= 150;
00324         magX = (maveX - 168.75) / 32768.0f * 4800.0f;//[uT]に変換
00325         magY = (maveY - 18.75) / 32768.0f * 4800.0f;//[uT]に変換
00326         magZ = mz / 32768.0f * 4800.0f;//[uT]に変換
00327         pc.printf("%f,%f\n\r", magX, magY);
00328         //ID = IDcheck();
00329         //pc.printf("%f\n\r", ID);
00330         rad = atan2(magZ * sin(roll) - magY * cos(roll), magX * cos(pitch) + magY * sin(pitch) * sin(roll) + magZ * sin(pitch) * cos(roll));
00331         degree = -((int)(rad * 180.0 / 3.141592 + 270.0 - 7.5) % 360 - 360.0);
00332         /*
00333         if(degree < 0){
00334             degree += 360;    
00335         }
00336         */
00337         pc.printf("%d\n\r",(int)degree);
00338         __enable_irq();     //割り込み許可
00339 }