Diff: main.cpp

Revision:

4:cfaf33b2c97a

Parent:

3:efaa785283a6

Child:

5:4e79ad68f3e3

--- a/main.cpp	Mon Feb 29 18:36:45 2016 +0000
+++ b/main.cpp	Fri Mar 04 03:26:39 2016 +0000
@@ -1,9 +1,10 @@
 #include "mbed.h"
 #include "SDFileSystem.h"
+#include "math.h"
 
 Ticker timer;
 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
-Serial gps(p28, p27);       // tx, rx
+Serial gps(p13, p14);       // tx, rx
 Serial pc(USBTX, USBRX);    // tx, rx 
 PwmOut moterl(p21);//左モーター
 PwmOut moterr(p22);//右モーター
@@ -13,18 +14,27 @@
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
 DigitalOut janpa1(p19);//パラシュートの開きを検知
-DigitalIn janpa2(p20);//パラシュートの開きを検知
+DigitalIn janpa2(p20);//パラシュートの開きを検知 
 
+float palse=0.02;
 char gps_data[256];
 float longitude,latitude,gpstime,knot,angle;
+//緯度longitude　経度latitude,
 int i=0;
-char *gps_target = "$GPRMC,174813.000,A,3606.9349,N,13927.4196,E,0.42,221.65,280216,,,A*6D";
-float longitude_target,latitude_target,gpstime_target,knot_target,angle_target;
+char *gps_target = "$GPRMC,230412.000,A,3022.5297,N,13057.6164,E,0.00,168.46,030316,,,A*6D";
+float high_target = 7.0;
+float longitude_target,latitude_target,gpstime_target,knot_target,angle_target,high;
 int count=0;
+int gomidata1,gomidata2,gomidata3,gomidata4,gomidata5,gomidata6;
+float longitudegosa;
+float latitudegosa;
+int angle_targetmath = 0;
 
 void gps_rx()//GPSデータ受信割り込み
 {
+    //pc.printf("gps_rx");
     gps_data[i] = gps.getc();
+    //gps_data[i] = pc.getc();
     if( gps_data[i] == '$' )
     {
         //$から受信データを保持する
@@ -39,16 +49,23 @@
         {
             // $GPRMCで始ってればデータを分けて格納
             sscanf(gps_data,"$GPRMC,%f,A,%f,N,%f,E,%f,%f",&gpstime,&longitude,&latitude,&knot,&angle);
+            pc.printf("GPRMC %f %f %f %f %f \n",gpstime,longitude,latitude,knot,angle);
+        }
+        if(memcmp(gps_data, "$GPGGA",6) == 0)
+        {
+            // $GPGGAで始ってればデータを分けて格納
+            sscanf(gps_data,"$GPGGA,%d,%d,N,%d,E,%d,%d,%d,%f,M",&gomidata1,&gomidata2,&gomidata3,&gomidata4,&gomidata5,&gomidata6,&high);
+            pc.printf("high %f",high);
         }
         //データをSDに書き込み
-        mkdir("/sd/mydir", 0777);
-        FILE *fp = fopen("/sd/mydir/gpsdata.txt", "a");
+        FILE *fp = fopen("/sd/gpsdata.txt", "a");
         if(fp == NULL)
         {
             error("Could not open file for write\n");
         }
         fprintf(fp,"%s \n",gps_data);
         fclose(fp);
+        //free(fp);
         
         //PCにデータ送信
         pc.printf("%s \n",gps_data);
@@ -60,7 +77,7 @@
         i++;
         if(i==255)
         {
-            pc.printf("*** Error! ***\n");
+            printf("*** Error! ***\n");
         }
     }
 
@@ -74,50 +91,108 @@
         case 0:
             //パラシュートを焼き切るまで待つ
             //焼き切ったらcount+1
+            //緯度longitude　経度latitude,
+            //pc.printf("case0");
+            if( (high_target+1.0)>= high )
+            {
+                pc.printf("hight_START");
+                //左右のモーターを動かす
+                moterl.pulsewidth(palse);  //パルス幅
+                moterr.pulsewidth(palse);
+                //焼き切り開始
+                led1=1;
+                //fet3=1;
+                wait(80);//焼き切り時間1分
+                led1=0;
+                angle_target = 90-atan2(sin(latitude_target-latitude),cos(longitude)*tan(longitude_target)-sin(longitude)*cos(latitude_target-latitude));
+                if((angle_target+angle)>360){angle_targetmath = (int(angle_target)+int(angle))%360;}
+                else{angle_targetmath = angle_target+angle;}
+                pc.printf("%f %d \n",angle_target,angle_targetmath);
+                count++;
+            }
             break;
         case 1:
             //落ちてパラシュートを焼き切った後
-            if( (angle_target+3)>=angle || (angle_target-3)<=angle )//3は適当な数字。誤差範囲
+            pc.printf("case1");
+            if( (angle_targetmath+5)>=int(angle) && (angle_targetmath-5)<=int(angle) )//3は適当な数字。誤差範囲
             {
                 //角度が合ったらcount+1
+                count++;
+                
             }
             else
             {
                 //角度が合うまで回転
-                if(angle_target>=angle)
+                if(angle_targetmath>=angle)
                 {
                     //differ_angle=angle_target-angle;
                     //moterlを動かす
+                    moterl.pulsewidth(palse);
+                    pc.printf("L \n");
                 }
-                else if(angle>=angle_target)
+                else if(angle>=angle_targetmath)
                 {
                     //differ_angle=angle-angle_target;
                     //moterrを動かす
+                    moterr.pulsewidth(palse);
+                    pc.printf("R \n");
                 }
             }
             break;
         case 2:
+            //pc.printf("case2");
             //目標地点の近くまで走行
             //目標地点に近づいたらcount+1
+            //緯度longitude　経度latitude,            
+            longitudegosa = longitude_target-longitude;
+            latitudegosa = latitude_target-latitude;
+            
+            if( abs(longitudegosa)<=50 && abs(latitudegosa)<=50 )
+            {
+                //longitude =;
+                //latitude =;
+                count++;
+            }
+            else
+            {
+                pc.printf("front \n");
+                moterl.pulsewidth(palse);  //パルス幅
+                moterr.pulsewidth(palse);
+            }
             break;
         case 3:
+            pc.printf("case3");
             //動作を停止。割り込みも停止させ、回収待ち
             //moterl=moterr=0;
             //ngps.detach();
+            count++;
             break;
         default:
+            moterl.pulsewidth(0.0);
+            moterr.pulsewidth(0.0);
             break;
         }
 }
  
 int main()
 {
+    pc.printf("START! \n");
     gps.baud(9600);
     gps.attach(gps_rx, Serial::RxIrq);//ＧＰＳ割り込み
+    //pc.attach(gps_rx, Serial::RxIrq);
     sscanf(gps_target,"$GPRMC,%f,A,%f,N,%f,E,%f,%f",&gpstime_target,&longitude_target,&latitude_target,&knot_target,&angle_target);
-    
+    pc.printf("%f %f %f %f %f \n",gpstime_target,longitude_target,latitude_target,knot_target,angle_target);
+    janpa1=1;
+    while(janpa2!=0);
+    pc.printf("PINOK");
+    moterl.period(0.02);      // 周期
+    moterr.period(0.02); 
     while(1)
     {
+        //pc.putc(gps.getc());
+        //pc.printf(",");
+        //moterl.pulsewidth(0.0020);  //パルス幅
+        //moterr.pulsewidth(0.0020);
         move();
     }