Diff: direction.h

Revision:

6:6fe6e3554a46

Parent:

5:e1001bfc423a

Child:

7:74994694ec04

--- a/direction.h	Thu Oct 28 09:12:27 2021 +0000
+++ b/direction.h	Thu Oct 28 13:50:27 2021 +0000
@@ -1,7 +1,158 @@
 #include "mbed.h"
 #include "getGPS.h"
 #include "Servo.h"
-#include "math.h"
+ 
+// 球面三角法により、大円距離(メートル)を求める
+double distance(double lat1, double lng1, double lat2, double lng2){
+    // 円周率
+    const double pi = 3.14159265359;
+ 
+    // 緯度経度をラジアンに変換
+    double rlat1 = lat1 * pi / 180;
+    double rlng1 = lng1 * pi / 180;
+    double rlat2 = lat2 * pi / 180;
+    double rlng2 = lng2 * pi / 180;
+ 
+    // 2点の中心角(ラジアン)を求める
+    double a =
+      sin(rlat1) * sin(rlat2) +
+      cos(rlat1) * cos(rlat2) *
+      cos(rlng1 - rlng2);
+    double rr = acos(a);
+ 
+    // 地球赤道半径(メートル)
+    const double earth_radius = 6378140;
+ 
+    // 2点間の距離(メートル)
+    double distance = earth_radius * rr;
+ 
+    return distance;
+}
+
+
+
+//CanSatから見た目的地の角度theta(-180<=theta<=180で、角度は正面が0で反時計回り)の計算  
+float calculate_theta(float x_0,float y_0,float x_1,float y_1,float x_2,float y_2){
+    
+    //float x_0 ,y_0: 目的地 , float x_1 ,y_1: 現在地, float x_2 ,y_2: 20秒前の現在地
+    //theta_0:目的地の角度,theta_1:CanSatの角度theta:CanSatから見た目的地の角度(-180)   
+    //theta_0,theta_1は北が90、東が0
+    //theta:CanSatから見た目的地の角度(-180<=theta<=180で、角度は正面が0で反時計回り)   
+    
+    float theta_0,theta_1;
+    
+    if(x_0 == x_1 && x_1 == x_2){
+        
+        if(y_0 - y_1 > 0){
+            theta_0 =  90;
+            }
+        if(y_0 - y_1 < 0){
+            theta_0 = 270;
+            }
+        if(y_0 - y_1 == 0){
+            theta_0 = 0;
+            }
+        
+        if(y_1 - y_2 > 0){
+            theta_1 =  90;
+            }
+        if(y_1 - y_2 < 0){
+            theta_1 = 270;
+            }
+        if(y_1 - y_2 == 0){
+            theta_1 = 0;
+            }    
+    }
+        
+    if(x_0 == x_1 && x_1 != x_2){
+        theta_1 = atan((y_1 - y_2)/(x_1- x_2));  
+        
+        if(y_0 - y_1 > 0){
+            theta_0 =  90;
+            }
+            
+        if(y_0 - y_1 < 0){
+            theta_0 = 270;
+            }
+            
+        if(y_0 - y_1 == 0){
+            theta_0 = 0;
+            }
+    
+        if(y_1 - y_2 > 0 && x_1 - x_2 < 0){
+            theta_1 = theta_1 - 180;
+        }
+        if(y_1 - y_2 == 0 && x_1 - x_2 < 0){
+            theta_1 = 180;
+        }
+        if(y_1 - y_2 < 0 && x_1 - x_2 > 0){
+            theta_1 = theta_1 + 180;
+        }     
+    }
+    
+    if(x_0 != x_1 && x_1 == x_2){
+        
+        theta_0 = atan((y_0 - y_1)/(x_0 - x_1));  
+    
+        if(y_0 - y_1 > 0 && x_0 - x_1 < 0){
+            theta_0 = theta_0 - 180;
+        }
+        if(y_0 - y_1 == 0 && x_0 - x_1 < 0){
+            theta_0 = 180;
+        }
+        if(y_0 - y_1 < 0 && x_0 - x_1 > 0){
+            theta_0 = theta_0 + 180;
+        }
+        
+        if(y_1 - y_2 > 0){
+            theta_1 =  90;
+            }
+        if(y_1 - y_2 < 0){
+            theta_1 = 270;
+            }
+        if(y_1 - y_2 == 0){
+            theta_1 = 0;
+            }    
+    }
+    
+    else{
+        theta_0 = atan((y_0 - y_1)/(x_0 - x_1));
+        theta_1 = atan((y_1 - y_2)/(x_1- x_2));  
+    
+        if(y_0 - y_1 > 0 && x_0 - x_1 < 0){
+            theta_0 = theta_0 - 180;
+        }
+        if(y_0 - y_1 == 0 && x_0 - x_1 < 0){
+            theta_0 = 180;
+        }
+        if(y_0 - y_1 < 0 && x_0 - x_1 > 0){
+            theta_0 = theta_0 + 180;
+        }
+    
+        if(y_1 - y_2 > 0 && x_1 - x_2 < 0){
+            theta_1 = theta_1 - 180;
+        }
+        if(y_1 - y_2 == 0 && x_1 - x_2 < 0){
+            theta_1 = 180;
+        }
+        if(y_1 - y_2 < 0 && x_1- x_2 > 0){
+            theta_1 = theta_1 + 180;
+        } 
+    
+        
+    }
+    
+    float theta = theta_0 - theta_1;
+    if(theta < -180){
+      theta = 360 - theta;
+      }
+    if(theta > 180){
+      theta = -360 + theta;
+      }  
+      
+    return theta;
+}
+ 
    
 //停止
    void stop(){
@@ -116,7 +267,7 @@
         turn_left();
 }
 
-class direction
+class direction:Servo
 {   
     private: