main.cpp

00001 /*2019.8 能代打ち上げ実験　CanSat用プログラム*/
00002 /*動作試験も本番も全く動かすことなく終わってしまったため，予想と異なる動きをする可能性がある(サイドスラスターの動く無機など)．*/
00003 #include "mbed.h"                   
00004 #include "MPU9250.h"
00005 #include "math.h"
00006 #include "MSCFileSystem.h"
00007 
00008 /*重要な定数をここで定義する*/
00009 #define CNT_LAUNCH_MAIN_ACC 3.0     //発射判定するときの加速度の閾値
00010 #define CNT_LAUNCH_CONST 3      //発射判定するときの加速度の閾値を超えなければならない連続数
00011 #define CNT_SWIM_TIME       60  //強制的にSwimフェーズに移行させる時間の閾値　
00012 #define CNT_FIN_DISTANCE_M  100     //ゴール判定の距離の閾値[m]　
00013 #define NUM_CNT_MEDIAN      3       //中央値をとる個数　
00014 #define NUM_CNT_LAND_TEST   2      //発射判定時にメジアン値が乗り越えなければならない回数
00015 #define O_X 40.24164                        //目標地点のgps座標のx軸　(当日決定)
00016 #define O_Y 140.0108                        //目標地点のgps座標のy軸
00017 #define DETECTION_NUM       3       //検知に使うデータの数（配列の要素数として扱う）(あまり重要ではない)
00018 #define N_N 220//地磁気がこの値より大きければ北を指す
00019 #define MIDDLE_N 50//地磁気がこの値より大きければ北東か北西を指す
00020 #define S_S -200//地磁気がこの値より小さければを南を指す(本番では間違えてこことこの下の値を逆にしていた．)
00021 #define MIDDLE_S -50//地磁気がこの値より大きければ南東か南西を指す
00022 #define BIAS 9 //偏角
00023 #define M_PI 3.1415926//円周率
00024 #define IDO_PER 111120 //緯度一度分の距離
00025 #define KEDO_PER 84989 //軽度一度分の距離（緯度一度分の距離×cos緯度）　今回は秋田県（緯度40度）で考えた 
00026 
00027 
00028 
00029 enum Phaze{                         //フェーズ定義
00030     STANDBY,
00031     LEAVE_LAND,
00032     SWIM,
00033     FIN
00034 };
00035 
00036 enum direct{                  //  方向範囲を番号付けする（だけ）列挙子
00037     n_n,
00038     n_e,
00039     e_e,
00040     s_e,
00041     s_s,
00042     s_w,
00043     w_w,
00044     n_w,
00045 };
00046 
00047 enum right_left{                    //右回りか左回りか
00048     right,
00049     left,
00050     straight,
00051 };
00052 
00053 
00054 float sum = 0;//ここらへんはソースコードのコピペで，不必要なものもあるかもしれない．
00055 uint32_t sumCount = 0;
00056 char buffer[14];
00057 
00058 /*各種使用機器の定義*/
00059 Timer t;
00060 Timer timer_all;
00061 Timer second_timer;
00062 PwmOut suisin(p23);
00063 PwmOut servo_l(p21);//右左逆だったらここを入れ替えればいい 
00064 PwmOut servo_r(p22);
00065 MSCFileSystem msc("usb");
00066 Serial gps(p13,p14);//(tx,rx) gpsの使用するとしてmbedの13,14ピンを用いるという宣言
00067 MPU9250 mpu9250;
00068 Timer t_a;
00069 Serial pc(USBTX, USBRX); // tx, rx
00070 char gps_data[256];
00071 float gpstime,gpsido,gpskeido,dido,mido,dkeido,mkeido;
00072 char ns,ew;
00073 int ht,mt,st;//計算結果 時刻
00074 float ido,keido;//計算結果
00075 int gps_i;
00076 bool Sateliterock =0;
00077 int AmountSatelite;
00078 int dummy;
00079 DigitalOut L1(LED1);
00080 
00081 
00082 
00083 /*自作関数*/
00084 /*関数を多用することで，実行動作の部品化やデバッグの効率化を図る*/
00085 
00086 float _DMS2DEG(float raw_data)
00087 /*gps_catchのための関数（60進数->100進数）*/
00088 {
00089     int d=(int)(raw_data/100);
00090     float m=(raw_data-(float)d*100);
00091     return (float)d+m/60;
00092 }
00093 
00094 void GPS()
00095 {
00096     
00097       if( (gps_data[gps_i]=gps.getc()) != '$'){
00098        // pc.printf("*** Error! ***\r\n");
00099         gps_i++;
00100         if(gps_i==255){
00101        //    pc.printf("*** Error! ***\r\n");
00102             gps_data[gps_i]='\0';
00103             
00104          }//if
00105     }else{ gps_data[gps_i]='\0';
00106       if(sscanf(gps_data, "GPGGA,%f,%f,%c,%f,%c,%d,%d",&gpstime,&gpsido,&ns,&gpskeido,&ew,&dummy,&AmountSatelite) >= 1){
00107               //hokui
00108           dido=int(gpsido/100);mido=(gpsido-dido*100)/60;ido=dido+mido;
00109          //tokei
00110           dkeido= int(gpskeido/100);mkeido=(gpskeido-dkeido*100)/60;keido=dkeido+mkeido;
00111             //time set
00112             ht=int(gpstime/10000);mt=int((gpstime-ht*10000)/100);st=int(gpstime-ht*10000-mt*100);ht=ht+9;//UTC =>JST
00113             if(AmountSatelite>=1)L1=1;;
00114     
00115      }
00116       gps_i=0;
00117     
00118     sprintf(gps_data,"");
00119       }
00120       
00121 }
00122 
00123 void gps_catch(float *x,float *y)
00124 /*引数x,yにそれぞれ緯度経度を代入する(下の方のwaitは邪魔かもしれない)*/
00125 {
00126     int i=0;
00127     gps.attach(GPS, Serial::RxIrq);
00128     while(i<5){
00129         //pc.printf("ido = %f , keido = %f\r\nns -> %c , ew -> %c\r\n",ido,keido,ns,ew);
00130         *x = ido;
00131         *y = keido;
00132         wait(0.5);
00133         i++;
00134     }    
00135 }
00136 
00137 float _median(float *data, int num)
00138 /*有効要素数numの配列data[]の中央値（メジアン）を返す関数 */
00139 {
00140     float ans;
00141     for(int i=0; i<num-1; i++){
00142         for(int j=0; j<num-1-i; j++){
00143             if(data[j]>data[j+1]){
00144                 float buff = data[j+1];
00145                 data[j+1] = data[j];
00146                 data[j] = buff;
00147             }
00148         }
00149     }
00150 
00151     if(num%2!=0) ans = data[num/2];
00152     else         ans = (data[num/2-1]+data[num/2])/2.0;
00153     return ans;
00154 }
00155 
00156 int p_vector_catch()
00157 /*地磁気データを受信し自分の方向を返すint型関数(この関数は0.5の遅れを生んでしまう．改善が必要)*/
00158 {
00159     pc.printf("p_vector\r\n");
00160     float _mx[NUM_CNT_MEDIAN],_my[NUM_CNT_MEDIAN],_mz[NUM_CNT_MEDIAN];
00161     int p_vector,i;
00162     //pc.printf("\n\n\n\n\n\n\n\n\n\n\n");
00163     pc.baud(9600);  
00164     i2c.frequency(400000);
00165     t.start();        
00166     
00167     uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
00168     if (whoami == 0x71)
00169     {  
00170         sprintf(buffer, "0x%x", whoami);
00171         wait(1);
00172         
00173         mpu9250.resetMPU9250();
00174         mpu9250.MPU9250SelfTest(SelfTest); 
00175         mpu9250.calibrateMPU9250(gyroBias, accelBias);
00176         mpu9250.initMPU9250(); 
00177         mpu9250.initAK8963(magCalibration);
00178     }
00179 
00180         mpu9250.getMres();
00181 
00182     while(1){
00183         whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
00184         if(whoami != 0x71){
00185             pc.printf("Could not connect to MPU9250: \n\r");
00186             pc.printf("%#x \n",  whoami);
00187             sprintf(buffer, "WHO_AM_I 0x%x", whoami);
00188             wait(1.0);
00189             continue;
00190         }
00191         if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
00192             for(i=0;i<NUM_CNT_MEDIAN;i++){
00193             
00194                 mpu9250.readMagData(magCount);
00195                 _mx[i] = (float)magCount[0]*mRes*magCalibration[0] -220;//- magbias[0];
00196                 _my[i] = (float)magCount[1]*mRes*magCalibration[1] -25;//- magbias[1];  
00197                 _mz[i] = (float)magCount[2]*mRes*magCalibration[2] +220;//- magbias[2];
00198                 }
00199             mx = _median(_mx,NUM_CNT_MEDIAN);
00200             my = _median(_my,NUM_CNT_MEDIAN);
00201             mz = _median(_mz,NUM_CNT_MEDIAN);
00202             
00203         }
00204         Now = t.read_us();
00205         deltat = (float)((Now - lastUpdate)/1000000.0f) ;
00206         lastUpdate = Now;
00207             
00208         sum += deltat;
00209         sumCount++;
00210 
00211         float ax=0,ay=0,az=0,gx=0,gy=0,gz=0;    
00212         mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
00213 
00214         delt_t = t.read_ms() - count;
00215         if (delt_t > 500) {//←ここがいらない
00216             pc.printf("mx = %f my = %f mz = %f  mG  \r\n", mx,my,mz);
00217             /*y軸がどこを指しているかで場合分け(機体の向きを算出する)*/
00218             if(N_N < my){//機体が真北を向いている
00219                 p_vector = n_n;    
00220             }else if(MIDDLE_N < my){
00221                 if(mx < 0){//機体が北西を向いている(x軸とy軸の関係を考慮して場合分けしてる)(gpsを裏返しているのでこの通りになる)
00222                     p_vector = n_e;
00223                 }else{//機体が北東を向いている
00224                     p_vector = n_w;
00225                 }
00226             }else if(MIDDLE_S < my){
00227                 if(mx < 0){//機体が真西を向いている
00228                     p_vector = e_e;
00229                 }else{//機体が真東を向いている
00230                     p_vector = w_w;
00231                 }
00232             }else if(S_S < my){
00233                 if(mx < 0){//機体が南西を向いている
00234                     p_vector = s_e;
00235                 }else{//機体が南東を向いている
00236                     p_vector = s_w;
00237                 }
00238             }else{//機体が真南を向いている
00239                 p_vector = s_s;
00240             }
00241             break;
00242         }
00243     }
00244     return p_vector;
00245 }
00246 
00247 void usb_write_data(int time,float gps_x,float gps_y)
00248 /*usbに1,2のデータを書き込むvoid型関数*/
00249 {
00250     FILE *fp= fopen("/usb/test.txt", "a");
00251     fprintf(fp,"\n%d,",time);//ファイルはCSV表記で記録し，エクセルでインポート出来るようにする
00252     fprintf(fp,"%f,",gps_x);
00253     fprintf(fp,"%f,",gps_y);
00254 }
00255 
00256 int acc_catch()
00257 /*合成加速度を返す関数(他にもジャイロ・地磁気・姿勢などを扱えるようにソースコードをコメントとして残しておいた)*/
00258 {
00259     
00260         float acc_sum;
00261         // If intPin goes high, all data registers have new data
00262  
00263 
00264         if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)   // On interrupt, check if data ready interrupt
00265 
00266             mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values
00267             // Now we'll calculate the accleration value into actual g's
00268             ax = (float)accelCount[0]*aRes/0.98;// - accelBias[0];  // get actual g value, this depends on scale being set
00269             ay = (float)accelCount[1]*aRes/0.98;// - accelBias[1];
00270             az = (float)accelCount[2]*aRes/0.98;// - accelBias[2];
00271  
00272             /* mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
00273             // Calculate the gyro value into actual degrees per second
00274             gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
00275             gy = (float)gyroCount[1]*gRes - gyroBias[1];
00276             gz = (float)gyroCount[2]*gRes - gyroBias[2];
00277  
00278             mpu9250.readMagData(magCount);  // Read the x/y/z adc values
00279             // Calculate the magnetometer values in milliGauss
00280             // Include factory calibration per data sheet and user environmental corrections
00281             mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
00282             my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
00283             mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];*/
00284         
00285  
00286  
00287 //    if(lastUpdate - firstUpdate > 10000000.0f) {
00288 //     beta = 0.04;  // decrease filter gain after stabilized
00289 //     zeta = 0.015; // increasey bias drift gain after stabilized
00290 //   }
00291  
00292         // Pass gyro rate as rad/s
00293         //uint32_t us = t.read_us();
00294         //mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
00295         //us = t.read_us()-us;
00296 // mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
00297  
00298         // Serial print and/or display at 0.5 s rate independent of data rates
00299         // update LCD once per half-second independent of read rate
00300             pc.printf("ax = %f\t",ax);
00301             pc.printf("ay = %f\t",ay);
00302             pc.printf("az = %f\t",az);
00303             acc_sum = sqrtf(ax*ax + ay*ay + az*az);
00304             pc.printf("sum = %f mG\t",acc_sum);
00305  
00306             /*pc.printf("gx = %f", gx);
00307             pc.printf(" gy = %f", gy);
00308             pc.printf(" gz = %f  deg/s\n\r", gz);
00309  
00310             pc.printf("gx = %f", mx);
00311             pc.printf(" gy = %f", my);
00312             pc.printf(" gz = %f  mG\n\r", mz);
00313  
00314             tempCount = mpu9250.readTempData();  // Read the adc values
00315             temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
00316             pc.printf("temperature = %f  C\n\r", temperature);
00317  
00318             pc.printf("q0 = %f\n\r", q[0]);
00319             pc.printf("q1 = %f\n\r", q[1]);
00320             pc.printf("q2 = %f\n\r", q[2]);
00321             pc.printf("q3 = %f\n\r", q[3]);
00322  
00323  
00324  
00325             // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
00326             // In this coordinate system, the positive z-axis is down toward Earth.
00327             // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
00328             // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
00329             // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
00330             // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
00331             // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
00332             // applied in the correct order which for this configuration is yaw, pitch, and then roll.
00333             // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
00334             yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
00335             pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
00336             roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
00337             pitch *= 180.0f / PI;
00338             yaw   *= 180.0f / PI;
00339             yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
00340             roll  *= 180.0f / PI;
00341  
00342             pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
00343             pc.printf("average rate = %f\n\r", (float) sumCount/sum);*/
00344  
00345             myled= !myled;
00346     return acc_sum;
00347 }
00348 
00349 int flag_launch()
00350 /*発射検知する*/
00351 {
00352     int phaze,x,acc,constant=0;
00353     for(x=0;x<CNT_LAUNCH_CONST;x++){
00354         acc = acc_catch();
00355         if(acc < CNT_LAUNCH_MAIN_ACC){
00356             phaze = STANDBY;
00357             return phaze;
00358         }
00359     }
00360     phaze = LEAVE_LAND;
00361     return phaze;
00362 }
00363 
00364 int flag_land(int time_p)
00365 /*着水検知する*/
00366 {
00367     if(time_p > CNT_SWIM_TIME)
00368         return SWIM;
00369     return LEAVE_LAND;
00370 }
00371 
00372 int cal_object_vector(float dis_x,float dis_y)
00373 /*目標ベクトルを8つの方向変数の内から返す*/
00374 {
00375     int o_vector;
00376     if(dis_x == 0){//x方向で目標との距離がない場合
00377         if(0 < dis_y){//ベクトルがy方向に正の値を示す場合
00378             o_vector = n_n;//機体は北を向いているということ
00379         }else{//
00380             o_vector = s_s;//機体は南を向いているということ(x=0かつy=0となってもs_sを入れることになるが，実際はそうなる前にゴール判定が下されているはず)    
00381         }
00382     }else if(dis_y == 0){//y方向で目標との距離がない場合(以下同じように考える)
00383         if(0 < dis_x){
00384             o_vector = e_e;
00385         }else{
00386             o_vector = w_w;
00387         }
00388     }else if(dis_x < 0){
00389         dis_x *= -1;
00390         if(dis_y < 0){
00391             dis_y *= -1;
00392             if(4*dis_x < dis_y){
00393                 o_vector = s_s;
00394             }else if(4*dis_y < dis_x){
00395                 o_vector = w_w;
00396             }else{
00397                 o_vector = s_w;
00398             }
00399         }else{
00400             if(4*dis_x < dis_y){
00401                 o_vector = n_n;
00402             }else if(4*dis_y < dis_x){
00403                 o_vector = w_w;
00404             }else{
00405                 o_vector = n_w;
00406             }
00407         }
00408     }else{
00409         if(dis_y < 0){
00410             dis_y *= -1;
00411             if(4*dis_x < dis_y){
00412                 o_vector = s_s;
00413             }else if(4*dis_y < dis_x){
00414                 o_vector = e_e;
00415             }else{
00416                 o_vector = s_e;
00417             }
00418         }else{
00419             if(4*dis_x < dis_y){
00420                 o_vector = n_n;
00421             }else if(4*dis_y < dis_x){
00422                 o_vector = e_e;
00423             }else{
00424                 o_vector = n_e;
00425             }
00426         }
00427     }
00428     return o_vector;
00429 }
00430 
00431 int right_or_left(int o_vector,int p_vector)
00432 /*右回りか左回りか決定する関数*/
00433 {
00434     int r_l;
00435     int k = o_vector - p_vector;
00436 
00437     if(k == 0){
00438         r_l = straight;
00439     }else if(4 <= k){
00440         r_l = left;
00441     }else if(0 < k){
00442         r_l = right;
00443     }else if(-4 <= k){
00444         r_l = left;
00445     }else {
00446         r_l = right;
00447     }
00448     return r_l;
00449 }
00450 
00451 void moter_control(int r_l)
00452 /*モーターを正しい向きに切り替え制御機構発動*/
00453 {
00454     switch(r_l){
00455         case straight:
00456             servo_l = 0.0f;
00457             servo_r = 0.0f;
00458             wait(3.0);
00459             break;
00460         case right:
00461             servo_l = 0.0f;
00462             servo_r = 0.5f;
00463             wait(1.0);
00464             servo_l = 0.0f;
00465             servo_r = 0.0f;
00466             wait(2.0);
00467             break;
00468         case left:
00469             servo_l = 0.5f;
00470             servo_r = 0.0f;
00471             wait(1.0);
00472             servo_l = 0.0f;
00473             servo_r = 0.0f;
00474             wait(2.0);
00475             break;
00476     }
00477 }
00478 
00479 int flag_fin(int gps_x,int gps_y)
00480 /*ゴール検知*/
00481 {
00482     int distance_2,x,y,phaze;//distance_2は目標距離からの距離の２乗の値
00483     x = gps_x - O_X;
00484     x *= x;
00485     y = gps_y - O_Y;
00486     y *= y;
00487     distance_2 = x+y;//この後にdistanceをm^2になるように何かするべき
00488     pc.printf("distance = %f\r\n",sqrtf(distance_2));
00489 
00490     if(sqrtf(distance_2) < CNT_FIN_DISTANCE_M)
00491         phaze = FIN;
00492     else
00493         phaze = SWIM;
00494     return phaze;
00495 }
00496 
00497 void bundle_data(float *gps_x,float *gps_y,int time)
00498 /*　usbに書き込むデータを一括して管理する関数*/
00499 {
00500     usb_write_data(time,*gps_x,*gps_y);
00501     //wireless_print(*gps_x,*gps_y);
00502 }
00503 
00504 int main()                      //本体はここから
00505 {
00506     /*まずは部品の起動・設定*/
00507     FILE *fp= fopen("/usb/test3.txt", "a");
00508     pc.printf("\r\n\n\n\nstart\r\n");
00509     fprintf(fp,"a");
00510     pc.printf("a\r\n");
00511     int i,phaze;
00512     float gps_x=0.0,gps_y=0.0;
00513     int o_vector;                //目標方向(1~8)
00514     int p_vector;                //自分の向いている方向
00515     int r_or_l;           //方向制御で右回りだったら1左回りだったら0
00516     int time_p;              //今プロジェクトの重要な時間経過数
00517     float dis_x,dis_y;
00518 
00519     /*↓設定なしだとモーターは動き出してしまう*/
00520     suisin = 0.0f;
00521     servo_l = 0.0f;
00522     servo_r = 0.0f;
00523     timer_all.start();   //ここから時間経過を計測
00524 
00525     phaze = STANDBY;       //STANBAYに移行する
00526 
00527     /*加速度センサ設定開始(よくわからない．おまじまいのようなもの)*/
00528     pc.baud(9600);
00529     i2c.frequency(400000);  // use fast (400 kHz) I2C
00530     //isrPin.rise(&mpuisr);
00531     uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
00532     if (whoami == 0x71) { // WHO_AM_I should always be 0x68 
00533         mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
00534         //mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
00535         mpu9250.initMPU9250();
00536         //mpu9250.initAK8963(magCalibration);
00537     } else {
00538         while(1);
00539     }
00540     mpu9250.getAres(); // Get accelerometer sensitivity
00541     //mpu9250.getGres(); // Get gyro sensitivity
00542     //mpu9250.getMres(); // Get magnetometer sensitivity
00543     /*加速度センサ設定終了*/
00544 
00545     while (phaze != FIN){  //ようやく本編開始
00546         switch(phaze){            
00547             case STANDBY:           //発射検知するまでがSTANDBYフェーズ
00548                 pc.printf("STANDBY now\r\n");
00549                 time_p = timer_all.read();
00550                 pc.printf("time_p = %d\r\n",time_p);
00551                 fprintf(fp,"\r\n%d,,,",time_p);
00552                 phaze = flag_launch();
00553                 if(phaze == LEAVE_LAND){
00554                     time_p = timer_all.read();
00555                     timer_all.reset();
00556                     fprintf(fp,"\r\nGo LEAVE_LAND phaze\n");
00557                 }
00558                 break;                
00559             case LEAVE_LAND:       //着水検知するまでがLEAVE_LANDフェーズ
00560                 //pc.printf("LEAVE_LAND now\r\n");
00561                 time_p = timer_all.read();
00562                 fprintf(fp,"\n%d,,,",time_p);
00563                 phaze = flag_land(time_p);
00564                 if(phaze == SWIM){
00565                     timer_all.reset();
00566                     fprintf(fp,"\nGo SWIM Phaze\n");
00567                 }
00568                 break;
00569             case SWIM:              //ゴールするまでがSWIMフェーズ
00570                 //pc.printf("SWIM now\r\n");
00571                 wait(1);//これいらない
00572                 suisin = 1.0f;//ここからゴールまでずっと推進用モーターを回し続ける
00573                 
00574                 time_p = timer_all.read();
00575                 gps_catch(&gps_x,&gps_y);
00576                 bundle_data(&gps_x,&gps_y,time_p);
00577                 dis_x = (O_X - gps_x)*IDO_PER;//x方向のゴールまでの実際の距離
00578                 dis_y = (O_Y - gps_y)*KEDO_PER;//y方向のゴールまでの実際の距離
00579                 o_vector = cal_object_vector(dis_x,dis_y);
00580                 p_vector = p_vector_catch();
00581                 
00582                 switch(p_vector){//自身の向いている方向を記載する
00583                     case n_n:
00584                         pc.printf("p:n_n\r\n");
00585                         fprintf(fp,",真北,");
00586                         break;
00587                     case n_e:
00588                         pc.printf("p:n_e\r\n");
00589                         fprintf(fp,",北東,");
00590                         break;
00591                     case e_e:
00592                         pc.printf("p:e_e\r\n");
00593                         fprintf(fp,",真東,");
00594                         break;
00595                     case s_e:
00596                         pc.printf("p:s_e\r\n");
00597                         fprintf(fp,",南東,");
00598                         break;
00599                     case s_s:
00600                         pc.printf("p:s_s\r\n");
00601                         fprintf(fp,",真南,");
00602                         break;
00603                     case s_w:
00604                         pc.printf("p:s_w\r\n");
00605                         fprintf(fp,",南西,");
00606                         break;
00607                     case w_w:
00608                         pc.printf("p:w_w\r\n");
00609                         fprintf(fp,",真西,");
00610                         break;
00611                     case n_w:
00612                         pc.printf("p:n_w\r\n");
00613                         fprintf(fp,",北西,");
00614                         break;
00615                 }
00616 
00617                 r_or_l = right_or_left(o_vector,p_vector);//どちらに向けばよいか定義する
00618                 moter_control(r_or_l);
00619                 phaze = flag_fin(dis_x,dis_y);
00620                 break;
00621             }
00622             //pc.printf("loop continue\r\n");
00623         }
00624     /*ゴールしたら全て閉じて終了*/
00625     suisin = 0.0f;
00626     pc.printf("finish");
00627     fprintf(fp,"\r\nFinish\r\n");
00628     fclose(fp);
00629     time_p = timer_all.read();
00630     timer_all.stop();
00631 }