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     /*int CP_num;           // CPリストのインデックス
00017     int last_num;         // CPリストの最後の要素のインデックス*/
00018     double GPS_x, GPS_y;  // 現在地の座標
00019     double direction;     // 次CPへの向き
00020     double CPs_x[3]={1, 2, 3};  // = [];  //CPリスト(x座標)
00021     double CPs_y[3]={1, 2, 3};  // = [];  // CPリスト(y座標)
00022     double next_CP_x, next_CP_y;
00023     
00024     // 落下検知
00025     // パラシュート分離
00026     /*
00027     wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
00028     while(flight_pin){
00029         pc.printf("flight_pin nuketa");
00030         wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
00031         nichrome=1;
00032         pc.printf("nichrome in");
00033         wait(30);
00034         nichrome=0;
00035     }
00036     // 落下終了
00037     */
00038     
00039     
00040     // 行動フロー開始
00041     //last_num = sizeof(CPs_x) / sizeof(double) - 1;  // ゴール地点のインデックスを算出
00042     while (next_CP_x != CPs_x[cp_max-1] && next_CP_y != CPs_y[cp_max-1]) { // ゴール判定
00043         for (int i = 0; i<=cp_max-1 ; i++) {
00044             // 移動
00045         next_CP_x = CPs_x[i];
00046         next_CP_y = CPs_y[i];
00047         
00048         pc.printf("i=%d\r\n", i);
00049         //catchGPS();
00050         pc.printf("\n\rlatitude=");
00051         pc.scanf("%lf",&gps.latitude);
00052         pc.printf("\n\rlongititude=");
00053         pc.scanf("%lf",&gps.longitude);
00054         pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
00055             
00056             while (1) {
00057                 speak();
00058                 /*
00059                 direction = AngleGet();
00060                 pc.printf("direction=%f\n", direction);
00061                 while(1) {
00062                     if(direction < 5 || direction > 355) {  //角度判定
00063                         Move('2', 1);
00064                         break;
00065                     }
00066                     else {
00067                         Move('1', 0);
00068                         Move('4', 0.5);
00069                     }
00070                 }
00071                 */
00072                 
00073                 if (FrontGet()) {
00074                     Move('1', 0);      //停止
00075                     Move('4', 0.5);    //回転
00076                     wait(1);
00077                     Move('1', 0);      //回転停止
00078                     continue;
00079                 } 
00080                 else {
00081                     Move('2', 0.5);
00082                 }
00083                 ///catchGPS();
00084                 pc.printf("\n\rlatitude=");
00085                 pc.scanf("%lf",&gps.latitude);
00086                 pc.printf("\n\rlongititude=");
00087                 pc.scanf("%lf",&gps.longitude);
00088                 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
00089                 if ((next_CP_x - gps.latitude)*(next_CP_x - gps.latitude) + (next_CP_y - gps.longitude)*(next_CP_y -gps.longitude) < 0.01) { // CP到着判定 //試験で調整
00090                     pc.printf("now leach cp[%d]=x_%f,y_%f\r\n",i,next_CP_x ,next_CP_y);
00091                     break;
00092                 }
00093                 
00094             }
00095         }
00096     // 行動フロー終了
00097     pc.printf("End\r\n");
00098     Move('1', 0);      //停止
00099     return 0;
00100     }
00101 }