Q-rover-Kai用計算ライブラリ 使用非推奨 動作未確認 あんまり意味なかった
Diff: Qcal.cpp
- Revision:
- 0:5ed58f4ef4fb
diff -r 000000000000 -r 5ed58f4ef4fb Qcal.cpp --- /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; +} + +