CanSat-C 2021 / Mbed 2 deprecated CanSat-C_test

統合試験用です。

Dependencies:   ATP3012 mbed TB6612FNG HMC US015 getGPS

main.cpp@7:8fab045d2616, 2021-10-26 (annotated)

Committer:
user_
Date:
Tue Oct 26 07:19:24 2021 +0000
Revision:
7:8fab045d2616
Parent:
6:1cda8471adc3
Child:
8:5fe1b4bbd108
flight pin tougou

Who changed what in which revision?

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