CanSat-C 2021 / Mbed 2 deprecated CanSat-C_test

統合試験用です。

Dependencies:   ATP3012 mbed TB6612FNG HMC US015 getGPS

main.cpp@14:1b5be519bd49, 2021-10-27 (annotated)

Committer:
ushiroji
Date:
Wed Oct 27 08:11:10 2021 +0000
Revision:
14:1b5be519bd49
Parent:
13:38c5ffe5873a
Child:
15:4779723a4f75
test

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