Revision 20:02afaa2186f3, committed 2021-11-04

Comitter:

ushiroji

Date:

Thu Nov 04 11:35:18 2021 +0000

Parent:

19:046524ac0985

Commit message:

add Calibration to Function.h

Changed in this revision

--- a/ATP3012.lib	Thu Oct 28 13:16:07 2021 +0000
+++ b/ATP3012.lib	Thu Nov 04 11:35:18 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/CanSat-C-2021/code/ATP3012/#fe313ebd4f3b
+https://os.mbed.com/teams/CanSat-C-2021/code/ATP3012/#6dd04e220a4c

--- a/Function.h	Thu Oct 28 13:16:07 2021 +0000
+++ b/Function.h	Thu Nov 04 11:35:18 2021 +0000
@@ -116,7 +116,7 @@
 
 double AngleGet() 
 {
-    double angle;
+    double angle = 0;
     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
     angle = compass.sample() / 10;
 
@@ -137,3 +137,26 @@
     wait(2);
     return delta;
 }
+
+int Calibration()
+{
+    pc.printf("calibration start\r\n");
+    compass.setCalibrationMode(0x43);
+    wait(60);
+    compass.setCalibrationMode(0x45);
+    pc.printf("calibration end\r\n");
+    while(1) {
+        if(gps.getgps()) {  //現在地取得
+        pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
+        *pGPS_x = gps.latitude;
+        *pGPS_y = gps.longitude;
+        break;
+        }
+        else {
+            pc.printf("No data\r\n");//データ取得に失敗した場合
+            wait(1);
+        }
+    }
+
+    return 0;
+}
\ No newline at end of file

--- a/main.cpp	Thu Oct 28 13:16:07 2021 +0000
+++ b/main.cpp	Thu Nov 04 11:35:18 2021 +0000
@@ -15,8 +15,8 @@
     // 変数宣言
     double GPS_x, GPS_y;  // 現在地の座標
     double direction;     // 次CPへの向き
-    double CPs_x[3]={34.545658, 34.545837, 34.546052};  //CPリスト(x座標)
-    double CPs_y[3]={135.503469, 135.503676, 135.503503};    // CPリスト(y座標)
+    double CPs_x[3]={34.54608391847546, 34.545845666047306, 34.545666059919796};  //CPリスト(x座標)
+    double CPs_y[3]={135.50338843400897, 135.50368659080985, 135.50347298593758};    // CPリスト(y座標)
     double next_CP_x, next_CP_y;
     
     // 落下検知
@@ -35,6 +35,7 @@
     
     
     // 行動フロー開始
+    Calibration();
     while (next_CP_x != CPs_x[cp_max-1] && next_CP_y != CPs_y[cp_max-1]) { // ゴール判定
         for (int i = 0; i<=cp_max-1 ; i++) {
             // 移動
@@ -42,7 +43,6 @@
         next_CP_y = CPs_y[i];
         
         pc.printf("i=%d\r\n", i);
-        catchGPS();
         pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
             
             while (1) {
@@ -73,7 +73,10 @@
                 }
                 catchGPS();
                 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
-                if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x) + (next_CP_y = GPS_y)*(next_CP_y = GPS_y) < 0.01) { // CP到着判定 //試験で調整
+                
+                double lati = 111132.8715;    //1度あたりの緯度の距離(m)
+                double longi = 91535.79099;    //1度あたりの経度の距離(m)
+                if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x)*lati*lati + (next_CP_y = GPS_y)*(next_CP_y = GPS_y)*longi*longi < 100) { // CP到着判定 //試験で調整
                     pc.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
                     break;
                 }