Diff: main.cpp

Revision:

8:01fdbe4e967a

Parent:

4:36a36b18ca30

Child:

9:674a1131da7b

--- a/main.cpp	Fri Aug 10 11:09:48 2018 +0000
+++ b/main.cpp	Fri Aug 10 18:09:43 2018 +0000
@@ -1,6 +1,7 @@
 /*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);
@@ -8,9 +9,21 @@
 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が返ってくる
@@ -44,50 +57,31 @@
 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() {
-    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);
-    }
+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;
@@ -181,4 +175,135 @@
     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);
+        }
+      } 
+    }
+  }
+}
+
+    //ヒューベニの公式で距離出す
+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(){
+     while(1){
+        int16_t mx, my, mz;
+        float magX, magY, magZ, mag;
+        int theta;
+        //磁気の値を取得し、方位判定
+        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;
+        // 角度の表示
+        pc.printf("%d\n\r", theta);
+    }
 }
\ No newline at end of file