Revision 0:1b7e9aa47a80, committed 2018-07-13

Comitter:

ponpoko1939

Date:

Fri Jul 13 15:02:55 2018 +0000

Commit message:

ver1???;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jul 13 15:02:55 2018 +0000
@@ -0,0 +1,178 @@
+#include "mbed.h"
+#include "math.h"
+
+#define PI 3.14159265359
+
+//ピンの初期化
+DigitalOut motor1(p21);
+DigitalOut motor2(p22);
+DigitalOut motor3(p23);
+DigitalOut motor4(p24);
+Serial gps(p28,p27);       // tx, rx…GPSとの通信に使用
+Serial pc(USBTX, USBRX);    // tx, rx
+AnalogIn sw1(p15);
+
+//グローバル変数群
+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;
+float dis;
+
+//プロトタイプ宣言
+void Brake();
+void Turn();
+void Return();
+void turnL();
+void turnR();
+void getGPS();
+float distance(float ,float);
+void arg(float, float);
+
+int main() {
+    
+}
+
+void Brake(){
+    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 turnL(){
+    motor1 = 1;
+    motor2 = 0;
+    motor3 = 0;
+    motor4 = 1;      
+}
+
+void turnR(){
+    motor1 = 0;
+    motor2 = 1;
+    motor3 = 1;
+    motor4 = 0;      
+}
+
+    //ヒューベニの公式で距離出す
+float distance(float g_hokui, float g_tokei){
+   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);
+   return distance2;
+}
+
+//編集中、pdfの方と比べて検討
+void arg(float g_hokui, float g_tokei){
+    
+    int turn;
+    float arg,radt,ido,keido;
+    
+    //緯度と経度をそれぞれの差を算出している
+    ido = x - g_hokui;
+    keido = y - g_tokei;
+    
+    arg = atan2(ido,keido);     //arctanの値を算出
+    
+    if(ido >= 0){
+        //第一象限
+        if(arg >= 0){
+            radt = (arg * 180.0) / PI;
+            turn = 0;   //右回り 
+        }
+        //第四象限
+        else if(arg < 0){
+            radt = ((arg + PI) * 180) /PI;
+            turn = 0;
+        }
+    }else {
+        //第二象限
+        if(arg >= 0){
+            radt = (arg * 180) / PI;
+            turn = 1;   //左回り
+        }
+        //第三象限
+        else if(arg < 0){
+            radt = ((arg + PI) * 180) /PI;
+            turn = 1;
+        }
+    }
+}
+
+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;
+          pc.printf("Lat:%.6f, Lon:%.6f\n\r",g_hokui, g_tokei);     //現在地のgpsデータ
+          pc.printf("ch_Lat:%.6f, ch_Lon:%.6f\n\r",old_hokui-g_hokui, old_tokei-g_tokei);   //前回計測してからの移動（gps）
+          //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)));   //別の距離の出し方
+          dis = distance(g_hokui, g_tokei);   //ここでヒュベニの公式を利用
+          old_hokui=g_hokui;
+          old_tokei=g_tokei;
+          
+          //スイッチを押すことで目標のgpsデータ更新
+          if(sw1>=0.5){
+              x=g_tokei;
+              y=g_hokui;
+              pc.printf("\n\tchange destination\n\n\r");
+            }
+        }
+        else{//gpsデータ未受信時
+          pc.printf("\n\rStatus:unLock(%d)\n\r",rlock);
+          //pc.printf("%s",gps_data);
+        }
+        sprintf(gps_data, "");
+      }//if 
+    }
+  }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Jul 13 15:02:55 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/a7c7b631e539
\ No newline at end of file