ループのテスト
Dependencies: ATP3012 mbed TB6612FNG HMC6352 US015 getGPS
main.cpp
- Committer:
- ushiroji
- Date:
- 2021-10-28
- Revision:
- 19:fb9b9795ad9e
- Parent:
- 18:057b39ca7bd1
File content as of revision 19:fb9b9795ad9e:
// ライブラリ #include "mbed.h" // 自作関数 #include "Function.h" #include "speak.h" // フライトピン・ニクロム線関係 DigitalIn flight_pin(A0); DigitalOut nichrome(D13); // #define cp_max 3//cpの数を自分で入れてね int main() { // 変数宣言 /*int CP_num; // CPリストのインデックス int last_num; // CPリストの最後の要素のインデックス*/ double GPS_x, GPS_y; // 現在地の座標 double direction; // 次CPへの向き double CPs_x[3]={1, 2, 3}; // = []; //CPリスト(x座標) double CPs_y[3]={1, 2, 3}; // = []; // 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[cp_max-1] && next_CP_y != CPs_y[cp_max-1]) { // ゴール判定 for (int i = 0; i<=cp_max-1 ; i++) { // 移動 next_CP_x = CPs_x[i]; next_CP_y = CPs_y[i]; pc.printf("i=%d\r\n", i); //catchGPS(); pc.printf("\n\rlatitude="); pc.scanf("%lf",&gps.latitude); pc.printf("\n\rlongititude="); pc.scanf("%lf",&gps.longitude); pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude); while (1) { speak(); /* direction = AngleGet(); pc.printf("direction=%f\n", direction); while(1) { if(direction < 5 || direction > 355) { //角度判定 Move('2', 1); break; } else { Move('1', 0); Move('4', 0.5); } } */ if (FrontGet()) { Move('1', 0); //停止 Move('4', 0.5); //回転 wait(1); Move('1', 0); //回転停止 continue; } else { Move('2', 0.5); } ///catchGPS(); pc.printf("\n\rlatitude="); pc.scanf("%lf",&gps.latitude); pc.printf("\n\rlongititude="); pc.scanf("%lf",&gps.longitude); pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude); if ((next_CP_x - gps.latitude)*(next_CP_x - gps.latitude) + (next_CP_y - gps.longitude)*(next_CP_y -gps.longitude) < 0.01) { // CP到着判定 //試験で調整 pc.printf("now leach cp[%d]=x_%f,y_%f\r\n",i,next_CP_x ,next_CP_y); break; } } } // 行動フロー終了 pc.printf("End\r\n"); Move('1', 0); //停止 return 0; } }