main.cpp

00001 // ライブラリ
00002 #include "mbed.h"
00003 
00004 // 自作関数
00005 #include "Function.h"
00006 #include "speak.h"
00007 
00008 // フライトピン・ニクロム線関係
00009 DigitalIn flight_pin(A0);
00010 DigitalOut nichrome(D13);
00011 // 
00012 #define cp_max 3    //CPの数を入力する
00013 
00014 int main() {
00015     // 変数宣言
00016     double GPS_x, GPS_y;  // 現在地の座標
00017     double direction;     // 次CPへの向き
00018     double CPs_x[3]={34.54608391847546, 34.545845666047306, 34.545666059919796};  //CPリスト(x座標)
00019     double CPs_y[3]={135.50338843400897, 135.50368659080985, 135.50347298593758};    // CPリスト(y座標)
00020     double next_CP_x, next_CP_y;
00021     
00022     // 落下検知
00023     // パラシュート分離
00024     
00025     wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
00026     while(flight_pin){
00027         pc.printf("flight_pin nuketa");
00028         wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
00029         nichrome=1;
00030         pc.printf("nichrome in");
00031         wait(30);
00032         nichrome=0;
00033     }
00034     // 落下終了
00035     
00036     
00037     // 行動フロー開始
00038     Calibration();
00039     while (next_CP_x != CPs_x[cp_max-1] && next_CP_y != CPs_y[cp_max-1]) { // ゴール判定
00040         for (int i = 0; i<=cp_max-1 ; i++) {
00041             // 移動
00042         next_CP_x = CPs_x[i];
00043         next_CP_y = CPs_y[i];
00044         
00045         pc.printf("i=%d\r\n", i);
00046         pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
00047             
00048             while (1) {
00049                 speak();
00050                 
00051                 direction = AngleGet();
00052                 pc.printf("direction=%f\n", direction);
00053                 while(1) {
00054                     if(direction < 5 || direction > 355) {  //角度判定
00055                         Move('2', 1);
00056                         break;
00057                     }
00058                     else {
00059                         Move('1', 0);
00060                         Move('4', 0.5);
00061                     }
00062                 }
00063                 
00064                 if (FrontGet()) {
00065                     Move('1', 0);      //停止
00066                     Move('4', 0.5);    //回転
00067                     wait(1);
00068                     Move('1', 0);      //回転停止
00069                     continue;
00070                 } 
00071                 else {
00072                     Move('2', 0.5);
00073                 }
00074                 catchGPS();
00075                 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
00076                 
00077                 double lati = 111132.8715;    //1度あたりの緯度の距離(m)
00078                 double longi = 91535.79099;    //1度あたりの経度の距離(m)
00079                 if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x)*lati*lati + (next_CP_y = GPS_y)*(next_CP_y = GPS_y)*longi*longi < 100) { // CP到着判定 //試験で調整
00080                     pc.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
00081                     break;
00082                 }
00083                 
00084             }
00085         }
00086     // 行動フロー終了
00087     pc.printf("End\r\n");
00088     Move('1', 0);      //停止
00089     return 0;
00090     }
00091 }