main.cpp

00001 #include "mbed.h"
00002 #include "SDFileSystem.h"
00003 #include "math.h"
00004 
00005 Ticker timer;
00006 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
00007 Serial gps(p13, p14);       // tx, rx
00008 Serial pc(USBTX, USBRX);    // tx, rx 
00009 PwmOut moterl(p21);//左モーター
00010 PwmOut moterr(p22);//右モーター
00011 DigitalOut led1(LED1);
00012 DigitalOut fet3(p23);//ニクロム線
00013 DigitalOut led2(LED2);
00014 DigitalOut led3(LED3);
00015 DigitalOut led4(LED4);
00016 DigitalOut janpa1(p19);//パラシュートの開きを検知
00017 DigitalIn janpa2(p20);//パラシュートの開きを検知 
00018 
00019 float palse=0.02;
00020 char gps_data[256];
00021 float longitude,latitude,gpstime,knot,angle;
00022 //緯度longitude　経度latitude,
00023 int i=0;
00024 char *gps_target = "$GPRMC,230412.000,A,3022.5297,N,13057.6164,E,0.00,168.46,030316,,,A*6D";
00025 float high_target = 7.0;
00026 float longitude_target,latitude_target,gpstime_target,knot_target,angle_target,high;
00027 int count=0;
00028 int gomidata1,gomidata2,gomidata3,gomidata4,gomidata5,gomidata6;
00029 float longitudegosa;
00030 float latitudegosa;
00031 int angle_targetmath = 0;
00032 
00033 void gps_rx()//GPSデータ受信割り込み
00034 {
00035     pc.printf("gps_rx");
00036     gps_data[i] = gps.getc();
00037     //gps_data[i] = pc.getc();
00038     if( gps_data[i] == '$' )
00039     {
00040         //$から受信データを保持する
00041         gps_data[0] = '$';
00042         i = 1;
00043     }
00044     else if( gps_data[i-1] == '\r' && gps_data[i] == '\n' )
00045     {
00046         //改行コードまでのデータを解析する
00047         gps_data[i+1] = '\0';
00048         if(memcmp(gps_data, "$GPRMC",6) == 0)
00049         {
00050             // $GPRMCで始ってればデータを分けて格納
00051             sscanf(gps_data,"$GPRMC,%f,A,%f,N,%f,E,%f,%f",&gpstime,&longitude,&latitude,&knot,&angle);
00052             pc.printf("GPRMC %f %f %f %f %f \n",gpstime,longitude,latitude,knot,angle);
00053         }
00054         if(memcmp(gps_data, "$GPGGA",6) == 0)
00055         {
00056             // $GPGGAで始ってればデータを分けて格納
00057             sscanf(gps_data,"$GPGGA,%d,%d,N,%d,E,%d,%d,%d,%f,M",&gomidata1,&gomidata2,&gomidata3,&gomidata4,&gomidata5,&gomidata6,&high);
00058             pc.printf("high %f",high);
00059         }
00060         //データをSDに書き込み
00061         FILE *fp = fopen("/sd/gpsdata.txt", "a");
00062         if(fp == NULL)
00063         {
00064             error("Could not open file for write\n");
00065         }
00066         fprintf(fp,"%s \n",gps_data);
00067         fclose(fp);
00068         //free(fp);
00069         
00070         //PCにデータ送信
00071         pc.printf("%s \n",gps_data);
00072         i = 0;
00073     }
00074     else
00075     {
00076         //改行コードが来るまでカウントを続ける
00077         i++;
00078         if(i==255)
00079         {
00080             printf("*** Error! ***\n");
00081         }
00082     }
00083 
00084 }
00085 
00086 void move()
00087 {
00088     //動作
00089     switch (count)
00090     {
00091         case 0:
00092             //パラシュートを焼き切るまで待つ
00093             //焼き切ったらcount+1
00094             //緯度longitude　経度latitude,
00095             //pc.printf("case0");
00096             if( (high_target+1.0)>= high )
00097             {
00098                 pc.printf("hight_START");
00099                 //左右のモーターを動かす
00100                 moterl.pulsewidth(palse);  //パルス幅
00101                 moterr.pulsewidth(palse);
00102                 //焼き切り開始
00103                 led1=1;
00104                 //fet3=1;
00105                 wait(80);//焼き切り時間1分
00106                 led1=0;
00107                 angle_target = 90-atan2(sin(latitude_target-latitude),cos(longitude)*tan(longitude_target)-sin(longitude)*cos(latitude_target-latitude));
00108                 if((angle_target+angle)>360){angle_targetmath = (int(angle_target)+int(angle))%360;}
00109                 else{angle_targetmath = angle_target+angle;}
00110                 pc.printf("%f %d \n",angle_target,angle_targetmath);
00111                 count++;
00112             }
00113             break;
00114         case 1:
00115             //落ちてパラシュートを焼き切った後
00116             pc.printf("case1");
00117             if( (angle_targetmath+5)>=int(angle) && (angle_targetmath-5)<=int(angle) )//3は適当な数字。誤差範囲
00118             {
00119                 //角度が合ったらcount+1
00120                 count++;
00121                 
00122             }
00123             else
00124             {
00125                 //角度が合うまで回転
00126                 if(angle_targetmath>=angle)
00127                 {
00128                     //differ_angle=angle_target-angle;
00129                     //moterlを動かす
00130                     moterl.pulsewidth(palse);
00131                     pc.printf("L \n");
00132                 }
00133                 else if(angle>=angle_targetmath)
00134                 {
00135                     //differ_angle=angle-angle_target;
00136                     //moterrを動かす
00137                     moterr.pulsewidth(palse);
00138                     pc.printf("R \n");
00139                 }
00140             }
00141             break;
00142         case 2:
00143             //pc.printf("case2");
00144             //目標地点の近くまで走行
00145             //目標地点に近づいたらcount+1
00146             //緯度longitude　経度latitude,            
00147             longitudegosa = longitude_target-longitude;
00148             latitudegosa = latitude_target-latitude;
00149             
00150             if( abs(longitudegosa)<=50 && abs(latitudegosa)<=50 )
00151             {
00152                 //longitude =;
00153                 //latitude =;
00154                 count++;
00155             }
00156             else
00157             {
00158                 pc.printf("front \n");
00159                 moterl.pulsewidth(palse);  //パルス幅
00160                 moterr.pulsewidth(palse);
00161             }
00162             break;
00163         case 3:
00164             pc.printf("case3");
00165             //動作を停止。割り込みも停止させ、回収待ち
00166             //moterl=moterr=0;
00167             //ngps.detach();
00168             count++;
00169             break;
00170         default:
00171             moterl.pulsewidth(0.0);
00172             moterr.pulsewidth(0.0);
00173             break;
00174         }
00175 }
00176  
00177 int main()
00178 {
00179     pc.printf("START! \n");
00180     gps.baud(9600);
00181     gps.attach(gps_rx, Serial::RxIrq);//ＧＰＳ割り込み
00182     //pc.attach(gps_rx, Serial::RxIrq);
00183     sscanf(gps_target,"$GPRMC,%f,A,%f,N,%f,E,%f,%f",&gpstime_target,&longitude_target,&latitude_target,&knot_target,&angle_target);
00184     pc.printf("%f %f %f %f %f \n",gpstime_target,longitude_target,latitude_target,knot_target,angle_target);
00185     janpa1=1;
00186     while(janpa2!=0);
00187     pc.printf("PINOK");
00188     moterl.period(0.02);      // 周期
00189     moterr.period(0.02); 
00190     while(1)
00191     {
00192         pc.putc(gps.getc());
00193         pc.printf(",");
00194         moterl.pulsewidth(0.0020);  //パルス幅
00195         moterr.pulsewidth(0.0020);
00196         //move();
00197     }
00198 
00199 }