Kotaro Yamamoto / Qcal

Q-rover-Kai用計算ライブラリ　使用非推奨　動作未確認　あんまり意味なかった

Qcal.cpp@0:5ed58f4ef4fb, 2017-12-07 (annotated)

Committer:
KOTAROYamamoto
Date:
Thu Dec 07 08:53:42 2017 +0000
Revision:
0:5ed58f4ef4fb
Q-rover-Kai??????????????????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
KOTAROYamamoto 0:5ed58f4ef4fb 1 #include "mbed.h"
KOTAROYamamoto 0:5ed58f4ef4fb 2 #include <math.h>
KOTAROYamamoto 0:5ed58f4ef4fb 3 #include "Qcal.h"/*
KOTAROYamamoto 0:5ed58f4ef4fb 4 #define float lat1 ;//緯度一度あたりの距離(m)
KOTAROYamamoto 0:5ed58f4ef4fb 5 #define float lon1 ;//経度一度あたりの距離(m)
KOTAROYamamoto 0:5ed58f4ef4fb 6
KOTAROYamamoto 0:5ed58f4ef4fb 7 #define float X ;//現在地−ゴール間の経度方向距離(m)
KOTAROYamamoto 0:5ed58f4ef4fb 8 #define float Y ;//現在地−ゴール間の緯度方向距離(m)
KOTAROYamamoto 0:5ed58f4ef4fb 9
KOTAROYamamoto 0:5ed58f4ef4fb 10 #define float X2 ;//現在地−ゴール間の経度方向距離(m)
KOTAROYamamoto 0:5ed58f4ef4fb 11 #define float Y2 ;//現在地−ゴール間の緯度方向距離(m)
KOTAROYamamoto 0:5ed58f4ef4fb 12
KOTAROYamamoto 0:5ed58f4ef4fb 13 #define float pan ;
KOTAROYamamoto 0:5ed58f4ef4fb 14 #define float dan ;
KOTAROYamamoto 0:5ed58f4ef4fb 15
KOTAROYamamoto 0:5ed58f4ef4fb 16 #define float an ;//理想回転角
KOTAROYamamoto 0:5ed58f4ef4fb 17 */
KOTAROYamamoto 0:5ed58f4ef4fb 18
KOTAROYamamoto 0:5ed58f4ef4fb 19
KOTAROYamamoto 0:5ed58f4ef4fb 20 QCal::QCal(float gn,float ge,float lat1,float lon1){
KOTAROYamamoto 0:5ed58f4ef4fb 21
KOTAROYamamoto 0:5ed58f4ef4fb 22 float X;//現在地−ゴール間の経度方向距離(m)
KOTAROYamamoto 0:5ed58f4ef4fb 23 float Y;//現在地−ゴール間の緯度方向距離(m)
KOTAROYamamoto 0:5ed58f4ef4fb 24
KOTAROYamamoto 0:5ed58f4ef4fb 25 float X2;//現在地−ゴール間の経度方向距離(m)
KOTAROYamamoto 0:5ed58f4ef4fb 26 float Y2;//現在地−ゴール間の緯度方向距離(m)
KOTAROYamamoto 0:5ed58f4ef4fb 27
KOTAROYamamoto 0:5ed58f4ef4fb 28 float pan;
KOTAROYamamoto 0:5ed58f4ef4fb 29 float dan;
KOTAROYamamoto 0:5ed58f4ef4fb 30
KOTAROYamamoto 0:5ed58f4ef4fb 31 float an;//理想回転角
KOTAROYamamoto 0:5ed58f4ef4fb 32 }
KOTAROYamamoto 0:5ed58f4ef4fb 33
KOTAROYamamoto 0:5ed58f4ef4fb 34 float QCal::QCalculate(float X,float Y,float mx,float my){
KOTAROYamamoto 0:5ed58f4ef4fb 35 float PI = 3.1416;
KOTAROYamamoto 0:5ed58f4ef4fb 36 pan = (atan2(X,Y)*180)/PI;//位置角(°)panを定義
KOTAROYamamoto 0:5ed58f4ef4fb 37 dan = (atan2(mx,my)*180)/PI;//方位角(°)danを定義
KOTAROYamamoto 0:5ed58f4ef4fb 38
KOTAROYamamoto 0:5ed58f4ef4fb 39 if(fabs(180-(pan-dan))-180>0){
KOTAROYamamoto 0:5ed58f4ef4fb 40 an = (180-fabs(pan-dan));
KOTAROYamamoto 0:5ed58f4ef4fb 41 }
KOTAROYamamoto 0:5ed58f4ef4fb 42
KOTAROYamamoto 0:5ed58f4ef4fb 43 else if(fabs(180-(pan-dan))-180<0){
KOTAROYamamoto 0:5ed58f4ef4fb 44 an = fabs(pan-dan)-180;
KOTAROYamamoto 0:5ed58f4ef4fb 45 }
KOTAROYamamoto 0:5ed58f4ef4fb 46
KOTAROYamamoto 0:5ed58f4ef4fb 47 return an;
KOTAROYamamoto 0:5ed58f4ef4fb 48 }
KOTAROYamamoto 0:5ed58f4ef4fb 49
KOTAROYamamoto 0:5ed58f4ef4fb 50 float QCal::QCalculateRed(float X,float Y,float X2,float Y2){
KOTAROYamamoto 0:5ed58f4ef4fb 51 float PI = 3.1416;
KOTAROYamamoto 0:5ed58f4ef4fb 52 pan = (atan2(X2,Y2)*180)/PI;//位置角(°)panを定義
KOTAROYamamoto 0:5ed58f4ef4fb 53 dan = (atan2(X2-X,Y2-Y)*180)/PI;//方位角(°)danを定義
KOTAROYamamoto 0:5ed58f4ef4fb 54
KOTAROYamamoto 0:5ed58f4ef4fb 55 if(fabs(180-(pan-dan))-180>0){
KOTAROYamamoto 0:5ed58f4ef4fb 56 an = (180-fabs(pan-dan));
KOTAROYamamoto 0:5ed58f4ef4fb 57 }
KOTAROYamamoto 0:5ed58f4ef4fb 58
KOTAROYamamoto 0:5ed58f4ef4fb 59 else if(fabs(180-(pan-dan))-180<0){
KOTAROYamamoto 0:5ed58f4ef4fb 60 an = fabs(pan-dan)-180;
KOTAROYamamoto 0:5ed58f4ef4fb 61 }
KOTAROYamamoto 0:5ed58f4ef4fb 62
KOTAROYamamoto 0:5ed58f4ef4fb 63 return an;
KOTAROYamamoto 0:5ed58f4ef4fb 64 }
KOTAROYamamoto 0:5ed58f4ef4fb 65
KOTAROYamamoto 0:5ed58f4ef4fb 66