CanSat-C 2021 / Mbed 2 deprecated loop_test

ループのテスト

Dependencies:   ATP3012 mbed TB6612FNG HMC6352 US015 getGPS

main.cpp@8:5fe1b4bbd108, 2021-10-26 (annotated)

Committer:
user_
Date:
Tue Oct 26 08:04:13 2021 +0000
Revision:
8:5fe1b4bbd108
Parent:
7:8fab045d2616
Child:
9:9221ef8d36a8
test

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