CanSat-C 2021 / Mbed 2 deprecated Run_test

走行試験用のプログラムです。

Dependencies:   mbed TB6612FNG HMC6352 US015 getGPS

main.cpp@0:0454d5b5905b, 2021-11-06 (annotated)

Committer:
ushiroji
Date:
Sat Nov 06 01:27:45 2021 +0000
Revision:
0:0454d5b5905b
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ushiroji 0:0454d5b5905b 1 #include "mbed.h"
ushiroji 0:0454d5b5905b 2 #include "Function.h"
ushiroji 0:0454d5b5905b 3
ushiroji 0:0454d5b5905b 4 int main()
ushiroji 0:0454d5b5905b 5 {
ushiroji 0:0454d5b5905b 6 //直進
ushiroji 0:0454d5b5905b 7 Move('2', 1); // 前進
ushiroji 0:0454d5b5905b 8 wait(10);
ushiroji 0:0454d5b5905b 9 Move('1', 0); // 停止
ushiroji 0:0454d5b5905b 10
ushiroji 0:0454d5b5905b 11 //回転
ushiroji 0:0454d5b5905b 12 Move('4', 0.5); // 時計回りに回転
ushiroji 0:0454d5b5905b 13 wait(10);
ushiroji 0:0454d5b5905b 14 Move('1', 0); // 停止
ushiroji 0:0454d5b5905b 15
ushiroji 0:0454d5b5905b 16 //FrontGetの距離調整
ushiroji 0:0454d5b5905b 17 while(1) {
ushiroji 0:0454d5b5905b 18 if (FrontGet()) {
ushiroji 0:0454d5b5905b 19 Move('1', 0); //停止
ushiroji 0:0454d5b5905b 20 Move('4', 0.5); //回転
ushiroji 0:0454d5b5905b 21 wait(1);
ushiroji 0:0454d5b5905b 22 Move('1', 0); //停止
ushiroji 0:0454d5b5905b 23 }
ushiroji 0:0454d5b5905b 24 else {
ushiroji 0:0454d5b5905b 25 Move('2', 1);
ushiroji 0:0454d5b5905b 26 }
ushiroji 0:0454d5b5905b 27 }
ushiroji 0:0454d5b5905b 28
ushiroji 0:0454d5b5905b 29 /* main
ushiroji 0:0454d5b5905b 30 double GPS_x, GPS_y; // 現在地の座標
ushiroji 0:0454d5b5905b 31 double direction; // 次CPへの向き
ushiroji 0:0454d5b5905b 32 double CPs_x[3]={34.54608391847546, 34.545845666047306, 34.545666059919796}; //CPリスト(x座標)
ushiroji 0:0454d5b5905b 33 double CPs_y[3]={135.50338843400897, 135.50368659080985, 135.50347298593758}; // CPリスト(y座標)
ushiroji 0:0454d5b5905b 34 double next_CP_x, next_CP_y;
ushiroji 0:0454d5b5905b 35 int cp_max = sizeof(CPs_x);
ushiroji 0:0454d5b5905b 36
ushiroji 0:0454d5b5905b 37 while (next_CP_x != CPs_x[cp_max-1] && next_CP_y != CPs_y[cp_max-1]) { // ゴール判定
ushiroji 0:0454d5b5905b 38 for (int i = 0; i<=cp_max-1 ; i++) {
ushiroji 0:0454d5b5905b 39 // 移動
ushiroji 0:0454d5b5905b 40 next_CP_x = CPs_x[i];
ushiroji 0:0454d5b5905b 41 next_CP_y = CPs_y[i];
ushiroji 0:0454d5b5905b 42
ushiroji 0:0454d5b5905b 43 pc.printf("i=%d\r\n", i);
ushiroji 0:0454d5b5905b 44 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
ushiroji 0:0454d5b5905b 45
ushiroji 0:0454d5b5905b 46 while (1) {
ushiroji 0:0454d5b5905b 47 // 角度調整
ushiroji 0:0454d5b5905b 48 direction = AngleGet();
ushiroji 0:0454d5b5905b 49 pc.printf("direction=%f\n", direction);
ushiroji 0:0454d5b5905b 50 while(1) {
ushiroji 0:0454d5b5905b 51 if(direction < 5 || direction > 355) { //角度判定
ushiroji 0:0454d5b5905b 52 Move('2', 1);
ushiroji 0:0454d5b5905b 53 break;
ushiroji 0:0454d5b5905b 54 }
ushiroji 0:0454d5b5905b 55 else {
ushiroji 0:0454d5b5905b 56 Move('1', 0);
ushiroji 0:0454d5b5905b 57 Move('4', 0.5);
ushiroji 0:0454d5b5905b 58 }
ushiroji 0:0454d5b5905b 59 }
ushiroji 0:0454d5b5905b 60
ushiroji 0:0454d5b5905b 61 //障害物判定
ushiroji 0:0454d5b5905b 62 if (FrontGet()) {
ushiroji 0:0454d5b5905b 63 Move('1', 0); //停止
ushiroji 0:0454d5b5905b 64 Move('4', 0.5); //回転
ushiroji 0:0454d5b5905b 65 wait(1);
ushiroji 0:0454d5b5905b 66 Move('1', 0); //回転停止
ushiroji 0:0454d5b5905b 67 continue;
ushiroji 0:0454d5b5905b 68 }
ushiroji 0:0454d5b5905b 69 else {
ushiroji 0:0454d5b5905b 70 Move('2', 0.5);
ushiroji 0:0454d5b5905b 71 }
ushiroji 0:0454d5b5905b 72
ushiroji 0:0454d5b5905b 73 //到着判定
ushiroji 0:0454d5b5905b 74 catchGPS();
ushiroji 0:0454d5b5905b 75 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
ushiroji 0:0454d5b5905b 76
ushiroji 0:0454d5b5905b 77 double lati = 111132.8715; //1度あたりの緯度の距離(m)
ushiroji 0:0454d5b5905b 78 double longi = 91535.79099; //1度あたりの経度の距離(m)
ushiroji 0:0454d5b5905b 79 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 < 100) { // CP到着判定 //試験で調整
ushiroji 0:0454d5b5905b 80 pc.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
ushiroji 0:0454d5b5905b 81 break;
ushiroji 0:0454d5b5905b 82 }
ushiroji 0:0454d5b5905b 83 }
ushiroji 0:0454d5b5905b 84 }
ushiroji 0:0454d5b5905b 85
ushiroji 0:0454d5b5905b 86 // 行動フロー終了
ushiroji 0:0454d5b5905b 87 pc.printf("End\r\n");
ushiroji 0:0454d5b5905b 88 Move('1', 0); //停止
ushiroji 0:0454d5b5905b 89 return 0;
ushiroji 0:0454d5b5905b 90 }
ushiroji 0:0454d5b5905b 91 */
ushiroji 0:0454d5b5905b 92 }