CanSat-C 2021 / Mbed 2 deprecated Run-GPS

座標判定用のプログラムです。 (モータードライバー＋GPS)

Dependencies:   ATP3012 mbed a HMC US015_2 getGPS

main.cpp@1:db6b55732559, 2021-12-03 (annotated)

Committer:
ushiroji
Date:
Fri Dec 03 08:27:23 2021 +0000
Revision:
1:db6b55732559
Parent:
0:99f4fe3e21c6 
first version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ushiroji 0:99f4fe3e21c6 1 #include "mbed.h"
ushiroji 1:db6b55732559 2 #include "Function.h" // 自作関数
ushiroji 1:db6b55732559 3 #define cp_max 2 //CPの数を入力する
ushiroji 1:db6b55732559 4
ushiroji 0:99f4fe3e21c6 5
ushiroji 0:99f4fe3e21c6 6 int main() {
ushiroji 1:db6b55732559 7 // 変数宣言
ushiroji 1:db6b55732559 8 double GPS_x, GPS_y; // 現在地の座標
ushiroji 1:db6b55732559 9 // double direction; // 次CPへの向き
ushiroji 1:db6b55732559 10 double CPs_x[cp_max]={34.545588426585454, 34.5454484832847}; //CPリスト(x座標)
ushiroji 1:db6b55732559 11 double CPs_y[cp_max]={135.50282053551706, 135.50262437335775}; // CPリスト(y座標)
ushiroji 1:db6b55732559 12 double next_CP_x, next_CP_y;
ushiroji 1:db6b55732559 13
ushiroji 1:db6b55732559 14
ushiroji 1:db6b55732559 15 catchGPS();
ushiroji 1:db6b55732559 16 xbee.printf("XBee Connected\r\n");
ushiroji 1:db6b55732559 17 xbee.printf("Fall point(lati,long)=(%lf , %lf)\r\n", gps.latitude, gps.longitude);
ushiroji 1:db6b55732559 18 for (int i = 0; i<=cp_max-1; i++) {//最後のcp=goalまで移動
ushiroji 1:db6b55732559 19 next_CP_x = CPs_x[i];
ushiroji 1:db6b55732559 20 next_CP_y = CPs_y[i];
ushiroji 1:db6b55732559 21 xbee.printf("next_i=%d\r\n", i);
ushiroji 1:db6b55732559 22 while(1) {
ushiroji 1:db6b55732559 23 Move('2', 0.3);
ushiroji 1:db6b55732559 24 /*
ushiroji 1:db6b55732559 25 while(FrontGet()) {
ushiroji 1:db6b55732559 26 Move('1', 0); //停止
ushiroji 1:db6b55732559 27 Move('4', 0.3); //時計回り回転
ushiroji 1:db6b55732559 28 wait(0.5);
ushiroji 1:db6b55732559 29 Move('1', 0); //回転停止
ushiroji 1:db6b55732559 30 */
ushiroji 1:db6b55732559 31 catchGPS();
ushiroji 1:db6b55732559 32 double lati = 111132.8715; //1度あたりの緯度の距離(m)
ushiroji 1:db6b55732559 33 double longi = 91535.79099; //1度あたりの経度の距離(m)
ushiroji 1:db6b55732559 34 GPS_x = gps.latitude;
ushiroji 1:db6b55732559 35 GPS_y = gps.longitude;
ushiroji 1:db6b55732559 36 if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x)*lati*lati + (next_CP_y - GPS_y)*(next_CP_y - GPS_y)*longi*longi < 10) { // CP到着判定 //試験で調整
ushiroji 1:db6b55732559 37 xbee.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
ushiroji 1:db6b55732559 38 break;
ushiroji 1:db6b55732559 39 }
ushiroji 1:db6b55732559 40 }
ushiroji 0:99f4fe3e21c6 41 }
ushiroji 1:db6b55732559 42 xbee.printf("\n\rEnd\r\n");
ushiroji 1:db6b55732559 43 Move('1', 0); //停止
ushiroji 1:db6b55732559 44 return 0;
ushiroji 0:99f4fe3e21c6 45 }