Revision 0:5ed58f4ef4fb, committed 2017-12-07

Comitter:

KOTAROYamamoto

Date:

Thu Dec 07 08:53:42 2017 +0000

Commit message:

Q-rover-Kai??????????????????????????????

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Qcal.cpp	Thu Dec 07 08:53:42 2017 +0000
@@ -0,0 +1,66 @@
+#include "mbed.h"
+#include <math.h>
+#include "Qcal.h"/*
+#define float lat1 ;//緯度一度あたりの距離(m)
+#define float lon1 ;//経度一度あたりの距離(m)
+  
+#define float X ;//現在地−ゴール間の経度方向距離(m)
+#define float Y ;//現在地−ゴール間の緯度方向距離(m)
+  
+#define float X2 ;//現在地−ゴール間の経度方向距離(m)
+#define float Y2 ;//現在地−ゴール間の緯度方向距離(m)
+
+#define float pan ;
+#define float dan ;
+        
+#define float an ;//理想回転角
+*/
+
+
+QCal::QCal(float gn,float ge,float lat1,float lon1){
+
+        float X;//現在地−ゴール間の経度方向距離(m)
+        float Y;//現在地−ゴール間の緯度方向距離(m)
+  
+        float X2;//現在地−ゴール間の経度方向距離(m)
+        float Y2;//現在地−ゴール間の緯度方向距離(m)
+
+        float pan;
+        float dan;
+        
+        float an;//理想回転角
+}
+
+float QCal::QCalculate(float X,float Y,float mx,float my){
+    float PI = 3.1416;
+    pan = (atan2(X,Y)*180)/PI;//位置角(°)panを定義
+    dan = (atan2(mx,my)*180)/PI;//方位角(°)danを定義
+
+    if(fabs(180-(pan-dan))-180>0){
+    an = (180-fabs(pan-dan));
+    }
+    
+    else if(fabs(180-(pan-dan))-180<0){
+    an = fabs(pan-dan)-180;
+    }
+
+    return an;
+}
+
+float QCal::QCalculateRed(float X,float Y,float X2,float Y2){
+    float PI = 3.1416;
+    pan = (atan2(X2,Y2)*180)/PI;//位置角(°)panを定義
+    dan = (atan2(X2-X,Y2-Y)*180)/PI;//方位角(°)danを定義
+    
+    if(fabs(180-(pan-dan))-180>0){
+    an = (180-fabs(pan-dan));
+    }
+    
+    else if(fabs(180-(pan-dan))-180<0){
+    an = fabs(pan-dan)-180;
+    }    
+    
+    return an;
+}
+    
+        

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Qcal.h	Thu Dec 07 08:53:42 2017 +0000
@@ -0,0 +1,37 @@
+/***********
+*絶対にQ-rover-Kaiを50行以内で動かすという強い気持ち
+***********/
+#include "mbed.h"
+#include <math.h>
+#ifndef QCAL_H
+#define QCAL_H
+
+
+class QCal{
+    public:
+        QCal(float gn,float ge,float lat1,float lon1);
+        
+        float QCalculate(float X,float Y,float mx,float my);
+        float QCalculateRed(float X,float Y,float X2,float Y2);
+        
+        float an;//理想回転角
+        
+        float lat1;//緯度一度あたりの距離(m)
+        float lon1;//経度一度あたりの距離(m)
+  
+        float X;//現在地−ゴール間の経度方向距離(m)
+        float Y;//現在地−ゴール間の緯度方向距離(m)
+  
+        float X2;//現在地−ゴール間の経度方向距離(m)
+        float Y2;//現在地−ゴール間の緯度方向距離(m)
+        
+    private:
+        float pan;
+        float dan;
+        
+};
+
+float QCalculate(float X,float Y,float mx,float my);
+float QCalculateRed(float X,float Y,float X2,float Y2);
+
+#endif
\ No newline at end of file