CanSat-C 2021 / Mbed 2 deprecated CanSat-C

調整中

Dependencies:   mbed HMC6352 US015 TB6612FNG2 getGPS ATP3011

main.cpp@13:38c5ffe5873a, 2021-10-27 (annotated)

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