CanSat-C 2021 / Mbed 2 deprecated loop_test

ループのテスト

Dependencies:   ATP3012 mbed TB6612FNG HMC6352 US015 getGPS

main.cpp@16:b5a60a976bf7, 2021-10-27 (annotated)

Committer:
ushiroji
Date:
Wed Oct 27 10:48:07 2021 +0000
Revision:
16:b5a60a976bf7
Parent:
15:4779723a4f75
Child:
17:1b6a84ab4433
Rotate -> judge angle

Who changed what in which revision?

UserRevisionLine numberNew contents of line
user_ 6:1cda8471adc3 1 // ライブラリ
ushiroji 0:5a1b52164bbe 2 #include "mbed.h"
ushiroji 14:1b5be519bd49 3
user_ 6:1cda8471adc3 4 // 自作関数
ushiroji 14:1b5be519bd49 5 #include "Function.h"
ushiroji 15:4779723a4f75 6 #include "speak.h"
ushiroji 0:5a1b52164bbe 7
user_ 7:8fab045d2616 8 // フライトピン・ニクロム線関係
user_ 7:8fab045d2616 9 DigitalIn flight_pin(A0);
user_ 7:8fab045d2616 10 DigitalOut nichrome(D13);
user_ 8:5fe1b4bbd108 11 //
ushiroji 0:5a1b52164bbe 12
ushiroji 0:5a1b52164bbe 13 int main() {
ushiroji 0:5a1b52164bbe 14 // 変数宣言
ushiroji 0:5a1b52164bbe 15 int CP_num; // CPリストのインデックス
user_ 6:1cda8471adc3 16 int last_num; // CPリストの最後の要素のインデックス
user_ 6:1cda8471adc3 17 double GPS_x, GPS_y; // 現在地の座標
user_ 6:1cda8471adc3 18 double direction; // 次CPへの向き
user_ 6:1cda8471adc3 19 double CPs_x[100]; // = []; //CPリスト(x座標)
user_ 6:1cda8471adc3 20 double CPs_y[100]; // = []; // CPリスト(y座標)
user_ 6:1cda8471adc3 21 double next_CP_x, next_CP_y;
user_ 6:1cda8471adc3 22
user_ 6:1cda8471adc3 23 // 落下検知
user_ 7:8fab045d2616 24 // パラシュート分離
ushiroji 14:1b5be519bd49 25 wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
ushiroji 14:1b5be519bd49 26 while(flight_pin){
user_ 7:8fab045d2616 27 pc.printf("flight_pin nuketa");
user_ 7:8fab045d2616 28 wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
user_ 7:8fab045d2616 29 nichrome=1;
user_ 7:8fab045d2616 30 pc.printf("nichrome in");
user_ 7:8fab045d2616 31 wait(30);
user_ 7:8fab045d2616 32 nichrome=0;
ushiroji 14:1b5be519bd49 33 }
user_ 7:8fab045d2616 34 // 落下終了
user_ 6:1cda8471adc3 35
user_ 7:8fab045d2616 36
ushiroji 0:5a1b52164bbe 37
user_ 6:1cda8471adc3 38 // 行動フロー開始
user_ 6:1cda8471adc3 39 last_num = sizeof(CPs_x) / sizeof(double) - 1; // ゴール地点のインデックスを算出
user_ 6:1cda8471adc3 40 while (next_CP_x != CPs_x[last_num] && next_CP_y != CPs_y[last_num]) { // ゴール判定
user_ 6:1cda8471adc3 41 int i;
user_ 6:1cda8471adc3 42 for (i = CP_num; last_num; i++) {
user_ 6:1cda8471adc3 43 // 移動
ushiroji 14:1b5be519bd49 44 catchGPS();
ushiroji 14:1b5be519bd49 45
ushiroji 14:1b5be519bd49 46 while (1) {
ushiroji 15:4779723a4f75 47 speak();
ushiroji 15:4779723a4f75 48
ushiroji 14:1b5be519bd49 49 direction = AngleGet();
ushiroji 14:1b5be519bd49 50 pc.printf("direction=%f", direction);
ushiroji 16:b5a60a976bf7 51 while(1) {
ushiroji 16:b5a60a976bf7 52 if(direction < 5 || direction > 355) { //角度判定
ushiroji 16:b5a60a976bf7 53 Move('2', 1);
ushiroji 16:b5a60a976bf7 54 break;
ushiroji 16:b5a60a976bf7 55 }
ushiroji 16:b5a60a976bf7 56 else {
ushiroji 16:b5a60a976bf7 57 Move('1', 0);
ushiroji 16:b5a60a976bf7 58 Move('4', 0.5);
ushiroji 16:b5a60a976bf7 59 }
ushiroji 14:1b5be519bd49 60 }
user_ 8:5fe1b4bbd108 61
ushiroji 14:1b5be519bd49 62 if (FrontGet()) {
ushiroji 14:1b5be519bd49 63 Move('1', 0); //停止
ushiroji 14:1b5be519bd49 64 Move('4', 0.5); //回転
ushiroji 14:1b5be519bd49 65 wait(1);
ushiroji 14:1b5be519bd49 66 Move('1', 0); //回転停止
ushiroji 14:1b5be519bd49 67 continue;
ushiroji 14:1b5be519bd49 68 }
ushiroji 14:1b5be519bd49 69 else {
ushiroji 14:1b5be519bd49 70 Move('2', 0.5);
ushiroji 14:1b5be519bd49 71 }
ushiroji 14:1b5be519bd49 72 catchGPS();
ushiroji 14:1b5be519bd49 73 if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x) + (next_CP_y = GPS_y)*(next_CP_y = GPS_y) < 5) { // CP到着判定 //試験で調整
ushiroji 14:1b5be519bd49 74 break;
ushiroji 14:1b5be519bd49 75 }
user_ 8:5fe1b4bbd108 76 }
ushiroji 3:74d0faefdd78 77 }
user_ 6:1cda8471adc3 78 // 行動フロー終了
user_ 6:1cda8471adc3 79 return 0;
ushiroji 14:1b5be519bd49 80 }
ushiroji 5:56eddb7b4a9e 81 }