main.cpp

00001 /*ライブラリ*/
00002 #include "mbed.h"
00003 
00004 // 自作関数
00005 #include "Function.h"
00006 
00007 // フライトピン・ニクロム線関係
00008 DigitalIn flight_pin(A0);
00009 DigitalOut nichrome(D13);
00010 // 
00011 #define cp_max 4    //CPの数を入力する
00012 
00013 int main() {
00014     // 変数宣言
00015     double GPS_x, GPS_y;  // 現在地の座標
00016     double direction;     // 次CPへの向き
00017     double CPs_x[cp_max]={34.54497251645555,34.545054367534235,34.54502498510097,34.545342036897935};  //CPリスト(x座標)
00018     double CPs_y[cp_max]={135.50877273927074,135.50837525002106,135.50798030878718,135.50778889521388};    // CPリスト(y座標)
00019     double next_CP_x, next_CP_y;
00020     
00021     // 落下検知
00022     // パラシュート分離
00023     
00024     wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
00025     while(flight_pin){}
00026         xbee.printf("flight_pin nuketa\n\r"); 
00027         wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
00028         nichrome=1;//  ここ変える
00029         xbee.printf("nichrome in\n\r");
00030         wait(10);
00031         nichrome=0;
00032     // 落下終了
00033     
00034     
00035     // 行動フロー開始
00036     Calibration();
00037     xbee.printf("XBee Connected\r\n");
00038     xbee.printf("Fall point(lati,long)=(%lf , %lf)\r\n", gps.latitude, gps.longitude);
00039     for (int i = 0; i<=cp_max-1 ; i++) {//最後のcp=goalまで移動
00040         next_CP_x = CPs_x[i];
00041         next_CP_y = CPs_y[i];
00042         
00043         xbee.printf("next_i=%d\r\n", i);
00044             
00045         while (1) {
00046                 while(FrontGet()) {
00047                     xbee.printf("frontget\n\r");
00048                     Move('2', 0.1);
00049                     wait(0.5);
00050                     Move('1', 0);      //停止
00051                     Move('4', 0.2);    //時計回り回転
00052                     wait(0.5);
00053                     /*Move('2', 0.17);  
00054                     wait(0.2);*/
00055                     Move('1', 0);      //回転停止
00056                     xbee.printf("front_avoid_rotate\n\r");
00057                 }
00058                 
00059                 //障害物よけて走ってから目的地に回頭、走らないと障害物に向くかも
00060                 Move('2', 0.2);  
00061                 wait(0.8);
00062                 Move('1', 0);  
00063                 //ちょっと走るのおわり
00064                 //走りながらanglegetできたら、止まらない
00065                 
00066                 catchGPS();
00067                 direction = AngleGet(next_CP_x,next_CP_y);
00068                 xbee.printf("\n\n\r----direction start-----\r\n");
00069                 xbee.printf("next_cp_x=%lf, next_cp_y=%lf\r\n",next_CP_x,next_CP_y);
00070                 //角度調節
00071                 while(1) {
00072                     if(direction < 5 || direction > 350) {
00073                         xbee.printf("\n-----direction finish-----\r\n");
00074                         Move('1', 0);   //停止
00075                         Move('2', 0.39);
00076                         xbee.printf("now_direction=%f\r\n", direction);
00077                         break;
00078                         }
00079                     else {
00080                         Move('4', 0.19);//時計回りに回転
00081                         xbee.printf("now_direction=%lf\r\n", direction);
00082                         direction = AngleGet(next_CP_x,next_CP_y);
00083                         }
00084                 }
00085                 catchGPS();
00086                 xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude);
00087                 
00088                 double lati = 111132.8715;    //1度あたりの緯度の距離(m)
00089                 double longi = 91535.79099;    //1度あたりの経度の距離(m)
00090                 GPS_x = gps.latitude;
00091                 GPS_y = gps.longitude;
00092                 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 < 12.25) { // CP到着判定 //試験で調整
00093                     xbee.printf("!!!!!!!!now leach cp[%d]=x_%f,y_%f!!!!!!!!!!!\r\n",i,next_CP_x ,next_CP_y);
00094                     break;
00095                 }
00096                 speak();            
00097             }//while(1){}
00098         }//for(){}
00099     // 行動フロー終了
00100     xbee.printf("End\r\n");
00101     Move('2', 0.17);  
00102     wait(0.2);
00103     Move('1', 0);      //停止
00104     return 0;
00105 }