ループのテスト
Dependencies: ATP3012 mbed TB6612FNG HMC6352 US015 getGPS
main.cpp
- Committer:
- ushiroji
- Date:
- 2021-10-27
- Revision:
- 15:4779723a4f75
- Parent:
- 14:1b5be519bd49
- Child:
- 16:b5a60a976bf7
File content as of revision 15:4779723a4f75:
// ライブラリ #include "mbed.h" // 自作関数 #include "Function.h" #include "speak.h" // フライトピン・ニクロム線関係 DigitalIn flight_pin(A0); DigitalOut nichrome(D13); // int main() { // 変数宣言 int CP_num; // CPリストのインデックス int last_num; // CPリストの最後の要素のインデックス double GPS_x, GPS_y; // 現在地の座標 double direction; // 次CPへの向き double CPs_x[100]; // = []; //CPリスト(x座標) double CPs_y[100]; // = []; // CPリスト(y座標) double next_CP_x, next_CP_y; // 落下検知 // パラシュート分離 wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん while(flight_pin){ pc.printf("flight_pin nuketa"); wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s nichrome=1; pc.printf("nichrome in"); wait(30); nichrome=0; } // 落下終了 // 行動フロー開始 last_num = sizeof(CPs_x) / sizeof(double) - 1; // ゴール地点のインデックスを算出 while (next_CP_x != CPs_x[last_num] && next_CP_y != CPs_y[last_num]) { // ゴール判定 int i; for (i = CP_num; last_num; i++) { // 移動 catchGPS(); while (1) { speak(); direction = AngleGet(); pc.printf("direction=%f", direction); if(direction < 5 || direction > 355) { //角度判定 Move('2', 1); } else { Rotate(direction); } if (FrontGet()) { Move('1', 0); //停止 Move('4', 0.5); //回転 wait(1); Move('1', 0); //回転停止 continue; } else { Move('2', 0.5); } catchGPS(); if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x) + (next_CP_y = GPS_y)*(next_CP_y = GPS_y) < 5) { // CP到着判定 //試験で調整 break; } } } // 行動フロー終了 return 0; } }