Revision 1:8a25883c423c, committed 2017-09-13

Comitter:

pyonta2017

Date:

Wed Sep 13 09:44:08 2017 +0000

Parent:

0:6ddf1386e71d

Commit message:

aa

Changed in this revision

diff -r 6ddf1386e71d -r 8a25883c423c main.cpp
--- a/main.cpp	Mon Sep 11 10:05:39 2017 +0000
+++ b/main.cpp	Wed Sep 13 09:44:08 2017 +0000
@@ -55,6 +55,7 @@
 unsigned char timer_set;
 unsigned char phase;
 unsigned char step;
+unsigned char pitch;
 
 //気圧///////
 MPL3115A2 APsensor(&i2c, &mp);
@@ -93,7 +94,7 @@
         }
         else{
             rela_max = max_alt -alt;
-            if(rela_max > 50)flag2 = flag2+1;
+            if(rela_max > 150)flag2 = flag2+1;
         }
     }    
     //着地判定
@@ -136,8 +137,8 @@
  
 void getGPS(){
   //目的地 ブラックロック修正
-    goal_tokei = 118.9833;
-    goal_hokui = 40.92493;
+    goal_tokei = 119.1218;
+    goal_hokui = 40.8797;
     c = gps.getc();
   if( c=='$' || i == 256){
     mode = 0;
@@ -188,7 +189,7 @@
           mp.printf("%s",gps_data);
           Mu2.printf("@DT02No\r\n");
           if(phase == Descending)getAltitude();
-          //if(phase == Moving)test_get_direction();
+          if(phase == Moving)step = jump;
         }
         sprintf(gps_data, "");
       }
@@ -307,6 +308,7 @@
 double bfy;
 double bfx;
 
+
 //3軸からheading算出
 void getheading3axis() {
     //加速度算出
@@ -318,6 +320,8 @@
     phi = atan2(aycal,azcal);
     //phi += M_PI;
     theta = atan2(-axcal,aycal*sin(phi)+azcal*cos(phi));
+    if(theta > 0)pitch = 1;
+    if(theta < 0)pitch = 0;
     //theta += M_PI;
     
     //地磁気算出
@@ -369,30 +373,31 @@
 void motor_drive(){
         controlmotor1.write(1);
         controlmotor2.write(0);
-        pin21.write(0.8);
+        pin21.write(1.0);
         //if(pin1)mp.printf("Yes!\r\n");
 }
 
 //溶断プログラム
+//最終チェックポイント////////////////////////////////
 void burning(){
     wait(5);
     myled1 = 1;
-    para = 1;
-    wait(1);
+    //para = 1;
+    wait(1);    
     myled1 = 0;
-    para = 0;
+    //para = 0;
     wait(5);
     myled2 = 1;
-    marker = 1;
+    //marker = 1;
     wait(1);
     myled2 = 0;
-    marker = 0;
+    //marker = 0;
     wait(5);
     myled3 = 1;
-    jumparm = 1;
+    //jumparm = 1;
     wait(1);
     myled3 = 0;
-    jumparm = 0;    
+    //jumparm = 0;    
 }
 //跳躍モーター駆動まいまいかわいい
 void jumping(){
@@ -462,7 +467,7 @@
     APsensor.setOffsetTemperature(10);
     APsensor.setOffsetPressure(-32);
     mkdir("/sd/mydir", 0777);
-    min_alt = 4000.0;
+    min_alt = 5000.0;
     Flightpin = 1;
     //高度初期値
     for(int h = 0; h < 10; h++){
@@ -473,9 +478,13 @@
     ini_alt = ini_alt/10;
     mp.printf("ini_alt:%d\r\n",ini_alt);
     int jump_count;
-    phase = Preparing;
-    wait(900);//開始15分待機
+    
+    //最終チェックポイント/////////////////////////////////////////
+    //phase = Preparing;
+    //wait(1500);//開始25分待機
+    //phase = Descending;
     //phase = Moving;
+    phase = Landing_Fusing;
     /////////////////////////////単機能////////
     
     //wait(5);
@@ -554,71 +563,7 @@
                 
     //}
     /////////////////////////    
-        
-    
-    /*
-    //////////////////////////安全試験用/////////
-    //振動試験
-    myled1 = 0;
-    //wait(5);//test
-    wait(900);//end to end
-    //静荷重
-    FILE *sl = fopen("/local/static2.txt", "a");
-    wait(10);
-    myled1 = 1;
-    wait(5);
-    timer.reset();
-    timer.start(); 
-    //while(val < 30000){ //test 
-    while(val < 600000){ //end to end
-        val = timer.read_ms();
-        adxl[0] = 0x32;
-        i2c.write(addr,adxl,1);
-        i2c.read(0xA7,adxl,6);
-        axout = (((int16_t )adxl[1]) << 8) | adxl[0];
-        ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
-        azout = (((int16_t )adxl[5]) << 8) | adxl[4];
-        //pc.printf("time:%d,X:%d,Y:%d,Z:%d\r\n",val,axout,ayout,azout);
-        fprintf(sl, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
-        wait_ms(200); 
-        //wait必要、ＳＤがbusyの状態で送っちゃってるんじゃない？
-    }
-    fclose(sl);
-    timer.reset();
-    timer.stop();
-    val = 0;
-    myled1 = 0;
-    //wait(5); //test
-    wait(960); //end to end
-        
-    //開傘衝撃
-    FILE *im = fopen("/local/impact2.txt", "a");
-    wait(10);
-    myled2 = 1;
-    timer.reset();
-    timer.start();
-    //while(val < 10000){ //test   
-    while(val < 30000){ //end to end
-        val = timer.read_ms();
-        adxl[0] = 0x32;
-        i2c.write(addr,adxl,1);
-        i2c.read(0xA7,adxl,6);
-        axout = (((int16_t )adxl[1]) << 8) | adxl[0];
-        ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
-        azout = (((int16_t )adxl[5]) << 8) | adxl[4];
-        fprintf(im, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
-        wait_ms(3); 
-        //wait必要、ＳＤがbusyの状態で送っちゃってるんじゃない？
-    }
-    fclose(im);
-    timer.reset();
-    timer.stop();
-    val = 0;
-    myled2 = 0;
-    //wait(30); //test
-    wait(600); //end to end
-    ///////////////////////////
-    */
+       
     //着地検知用timer
     timer.reset();
     timer.start();
@@ -631,7 +576,7 @@
             wait(0.5);
             val = timer.read();
             if(flag1 > 20) phase = Rising;
-            if( val > 600) phase = Rising; //計25分
+            if( val > 600) phase = Rising; //計35分
             break; 
         case Rising:
             myled1 = 0;
@@ -639,15 +584,19 @@
             getAltitude();
             wait(0.5);
             val = timer.read();
-            if(flag2 > 10) phase =  Descending;
-            if(val > 900) phase =  Descending; 
+            if(flag2 > 40) phase =  Descending;
+            if(val > 1800) phase =  Descending; //計55分
             break;    
         case Descending:
             val = timer.read();  
             myled2 = 0;
             myled3 = 1;
-            getGPS();
-                        
+            if(flag3 < 50){
+                getGPS();
+            }
+            else{
+                getAltitude();
+            }                        
             if ( val >1500){
                 timer.reset();
                 val = 0;
@@ -655,13 +604,17 @@
             }
             if (timer_set == 1){
                 val_total = 1500 + val;
-            }    
-            if ( flag3 >= 300){
-                if( val_total > 1800){
+            }
+            //最終チェックポイント//////////////////////////////////////////    
+            if ( flag3 >= 200){
+                if( val > 900){
                 phase = Landing_Fusing;
                 }
             }
-            if( val_total > 2400) phase = Landing_Fusing;//電源onから55分               
+            if( val_total >2400 ){
+                wait(600);
+                phase = Landing_Fusing;//電源onから75分
+            }               
             break;
         case Landing_Fusing:
             timer.reset();
@@ -669,16 +622,29 @@
             val = 0;
             myled3 = 0;
             myled4 = 1;
-            wait(30);
+            //wait(30);
             burning();
             wait(5);
             Mu2.printf("@DT04FIRE\r\n");
             for ( int gp; gp <= 10; gp++){
                 getGPS();
             }
-            wait(5);
+
+            wait(2);
             jumpmotor = 1;
-            wait(15);
+            wait(10);
+            jumpmotor = 0;
+            wait(2);
+            pin21.write(1.0);
+            controlmotor1.write(0);
+            controlmotor2.write(1);
+            wait(4);
+            pin21.write(0.0);
+            controlmotor1.write(0);
+            controlmotor2.write(0);
+            wait(3);
+            jumpmotor = 1;
+            wait(10);
             jumpmotor = 0;
             phase = Moving;           
             break;
@@ -692,7 +658,6 @@
             case get_goaldirection:
                 myled1 = 1;
                 getGPS();
-                //test_get_direction();
                 step = attitude_determination;
                 wait(1);
                 break;
@@ -707,6 +672,9 @@
                     getheading3axis();
                     wait(0.5);
                 }
+                timer.reset();
+                timer.stop();
+                val = 0;
                 getheading3axis();
                 step = calculate_direction;
                 wait(1);
@@ -739,6 +707,7 @@
                 else {
                     step = direction_control;
                 }
+                if(pitch == 1) step = direction_control;
                 wait(1);
                 break;
                 
@@ -751,10 +720,15 @@
                 timer.stop();
                 timer.start();
                 val = 0;
-                while(val < 2){
-                    val = timer.read();
-                    motor_drive();            
-                    }
+                pin21.write(1.0);
+                if(pitch == 1){
+                    controlmotor1.write(0);
+                    controlmotor2.write(1);
+                }else{
+                    controlmotor1.write(1);
+                    controlmotor2.write(0);
+                }
+                wait(3);            
                 controlmotor1.write(0);
                 controlmotor2.write(0);
                 pin21.write(0.0);
@@ -762,7 +736,7 @@
                 timer.stop();
                 val = 0;
                 //step = jump;
-                if(jump_count == 3){
+                if(jump_count == 2){
                     step = jump;
                 }
                 else{
@@ -776,7 +750,7 @@
                 myled1 = 1;
                 myled2 = 1;
                 wait(2);
-                FILE *fp = fopen("/sd/mydir/height1.txt", "a");
+                FILE *fp = fopen("/sd/mydir/height.txt", "a");
                 wait(1);
                 timer.reset();
                 timer.start();
@@ -785,23 +759,28 @@
                 Mu2.printf("@DT04JUMP\r\n");
                 while(val < 4000){
                     val = timer.read_ms();
-                    adxl[0] = 0x32;
-                    i2c.write(addr,adxl,1);
-                    i2c.read(0xA7,adxl,6);
-                    axout = (((int16_t )adxl[1]) << 8) | adxl[0];
-                    ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
-                    azout = (((int16_t )adxl[5]) << 8) | adxl[4];
-                    fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); 
-                    wait_ms(2);
+                    if(fp == NULL) {
+                        jumpmotor = 1;
+                    }
+                    else{
+                        adxl[0] = 0x32;
+                        i2c.write(addr,adxl,1);
+                        i2c.read(0xA7,adxl,6);
+                        axout = (((int16_t )adxl[1]) << 8) | adxl[0];
+                        ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
+                        azout = (((int16_t )adxl[5]) << 8) | adxl[4];
+                        fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); 
+                        wait_ms(2);
+                    }
                 }
                 jumpmotor = 0;
                 wait(1);
-                fclose(fp);
+                if(fp != NULL)fclose(fp);
                 myled3 = 1;
+                val = 0;
                 jump_count = 0;
                 wait(3);
                 step = get_goaldirection;
-                //step = direction_control;
                 break;
             }
         }