cansat_B 2019 / Mbed 2 deprecated GPSXBeeCompleted

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@5:9ff2f6cd54d2, 2019-12-08 (annotated)

Committer:
KINU
Date:
Sun Dec 08 13:28:08 2019 +0000
Revision:
5:9ff2f6cd54d2
Parent:
4:88f7837f947f 
GPSXBeecompleted

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