cansat_B 2019 / Mbed 2 deprecated GPSと走行の統合　１

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

gps.cpp@6:892eb32f6dc3, 2019-12-05 (annotated)

Committer:
saeichi
Date:
Thu Dec 05 00:17:03 2019 +0000
Revision:
6:892eb32f6dc3
Parent:
5:f3b266c917a6 
GPSGO;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
KINU 1:c142b1682312 1 #include "mbed.h"
KINU 1:c142b1682312 2 #include "getGPS.h"
KINU 1:c142b1682312 3 #include "math.h"
saeichi 6:892eb32f6dc3 4 #include "TB6612.h"
KINU 1:c142b1682312 5
KINU 1:c142b1682312 6 Serial pc(USBTX,USBRX);
KINU 2:639bfe0721a7 7 GPS gps (p28,p27);
saeichi 4:88f7837f947f 8 Serial xbee(p13,p14);
saeichi 6:892eb32f6dc3 9 TB6612 left(p25,p17,p16);
saeichi 6:892eb32f6dc3 10 TB6612 right(p26,p19,p18);
KINU 1:c142b1682312 11
KINU 1:c142b1682312 12
KINU 1:c142b1682312 13 int main() {
YUPPY 3:3cd62c194bd0 14 double a;
YUPPY 3:3cd62c194bd0 15 double b;
YUPPY 3:3cd62c194bd0 16 double distance;
saeichi 6:892eb32f6dc3 17 int i = 0;
saeichi 6:892eb32f6dc3 18
saeichi 6:892eb32f6dc3 19 pc.printf("GPS Start\n");
saeichi 6:892eb32f6dc3 20 xbee.printf("s\n");
KINU 2:639bfe0721a7 21 while(1) {
KINU 2:639bfe0721a7 22 if(gps.getgps()){
saeichi 6:892eb32f6dc3 23
saeichi 6:892eb32f6dc3 24 pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
saeichi 6:892eb32f6dc3 25 i ++;
saeichi 6:892eb32f6dc3 26 if(i<29){
saeichi 6:892eb32f6dc3 27 }else if(i == 30){
KINU 1:c142b1682312 28 a = gps.latitude;
KINU 1:c142b1682312 29 b = gps.longitude;
KINU 1:c142b1682312 30
saeichi 6:892eb32f6dc3 31 }else{
KINU 1:c142b1682312 32 break;
saeichi 6:892eb32f6dc3 33 }
KINU 2:639bfe0721a7 34 }else{
KINU 1:c142b1682312 35 pc.printf("NO DATA\r\n");//データ取得失敗
KINU 1:c142b1682312 36 wait(1);
KINU 1:c142b1682312 37 }
KINU 1:c142b1682312 38 }
saeichi 6:892eb32f6dc3 39
saeichi 6:892eb32f6dc3 40 left = 100; //左モーター１００％
saeichi 6:892eb32f6dc3 41 right = 100;//右モーター１００％
saeichi 6:892eb32f6dc3 42 wait(30);
saeichi 6:892eb32f6dc3 43 left = 0; //左モーター１０％
saeichi 6:892eb32f6dc3 44 right = 0;//右モーター１０％(左折)
saeichi 6:892eb32f6dc3 45 wait(15);
saeichi 6:892eb32f6dc3 46
KINU 2:639bfe0721a7 47 while(1){
KINU 2:639bfe0721a7 48 if(gps.getgps()) {
KINU 2:639bfe0721a7 49
YUPPY 3:3cd62c194bd0 50 pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示
KINU 2:639bfe0721a7 51
YUPPY 3:3cd62c194bd0 52 // 球面三角法により、大円距離(メートル)を求める
YUPPY 3:3cd62c194bd0 53 double c;
YUPPY 3:3cd62c194bd0 54 double d;
YUPPY 3:3cd62c194bd0 55 c = gps.latitude;
YUPPY 3:3cd62c194bd0 56 d = gps.longitude;
KINU 2:639bfe0721a7 57
YUPPY 3:3cd62c194bd0 58 const double pi = 3.14159265359; // 円周率
KINU 2:639bfe0721a7 59
KINU 2:639bfe0721a7 60 double ra = a * pi / 180;
KINU 2:639bfe0721a7 61 double rb = b * pi / 180; // 緯度経度をラジアンに変換
KINU 2:639bfe0721a7 62 double rc = c * pi / 180;
KINU 2:639bfe0721a7 63 double rd = d * pi / 180;
YUPPY 3:3cd62c194bd0 64
KINU 2:639bfe0721a7 65 double e = sin(ra) * sin(rc) + cos(ra) * cos(rc) * cos(rb - rd); // 2点の中心角(ラジアン)を求める
KINU 2:639bfe0721a7 66 double rr = acos(e);
KINU 2:639bfe0721a7 67
KINU 2:639bfe0721a7 68 const double earth_radius = 6378140; // 地球赤道半径(m)
KINU 2:639bfe0721a7 69
YUPPY 3:3cd62c194bd0 70 distance = earth_radius * rr; // 2点間の距離(m)
YUPPY 3:3cd62c194bd0 71
KINU 2:639bfe0721a7 72
KINU 2:639bfe0721a7 73 if (distance<5){
KINU 2:639bfe0721a7 74 }else{
KINU 1:c142b1682312 75 pc.printf("5m clear!");
saeichi 4:88f7837f947f 76 xbee.printf("5m clear!");
KINU 1:c142b1682312 77 break;
KINU 2:639bfe0721a7 78 }
KINU 2:639bfe0721a7 79
KINU 1:c142b1682312 80 }else{
saeichi 4:88f7837f947f 81 xbee.printf("NO DATA\r\n");//データ取得失
saeichi 4:88f7837f947f 82 pc.printf("NO DATA\r\n");
KINU 1:c142b1682312 83 wait(1);
KINU 1:c142b1682312 84 }
KINU 1:c142b1682312 85 }
KINU 1:c142b1682312 86 return 0;
KINU 2:639bfe0721a7 87 }
KINU 2:639bfe0721a7 88
KINU 2:639bfe0721a7 89
saeichi 6:892eb32f6dc3 90
saeichi 6:892eb32f6dc3 91
saeichi 6:892eb32f6dc3 92
saeichi 6:892eb32f6dc3 93
saeichi 6:892eb32f6dc3 94
saeichi 6:892eb32f6dc3 95
saeichi 6:892eb32f6dc3 96
saeichi 6:892eb32f6dc3 97
KINU 2:639bfe0721a7 98
KINU 2:639bfe0721a7 99
KINU 2:639bfe0721a7 100
KINU 2:639bfe0721a7 101
KINU 2:639bfe0721a7 102