Diff: main.cpp

Revision:

33:8d3757a0cd93

Parent:

32:c522d463c4fa

Child:

34:0e8f95f07612

--- a/main.cpp	Wed Dec 12 10:22:22 2018 +0000
+++ b/main.cpp	Fri Dec 14 05:46:43 2018 +0000
@@ -17,8 +17,9 @@
 
 MPU6050 mpu(p9, p10);                //(SDA,SCL)のピンをアサインしてね☆   加速度センサー
 
-DigitalIn flight(p21);      //フライトピン
-DigitalOut FET(p22);                //FET
+DigitalIn flight(p23);      //フライトピン
+DigitalOut SW(p22);         //トリガー用
+DigitalOut FET(p21);                //FET
 
 InterruptIn button1(p15);           //フォトインタラプタ
 InterruptIn button2(p18);
@@ -54,14 +55,19 @@
     
     printf("cansat start\r\n");
     
-  // FET = 0; 
-  /*   while(1) {
+    FET = 0;
+    SW = 1;  //p23をhigh(3.3V)にする。 
+     while(1) {
     if(flight==1) {
         wait(10);
         }
         
     else{
-        FET = 0;                       //FET、ニクロム線切断
+        if(flight==1) {
+        wait(10);
+        }
+        else{
+    /*    FET = 0;                       //FET、ニクロム線切断
         wait(20);
         FET = 1;
         wait(12);
@@ -69,19 +75,21 @@
         wait(10);
         FET = 1;
         wait(12);
-        FET = 0; 
+        FET = 0; */
+        SW = 0;  //p23をlow(0V)にする。 
     break;
             }
-    }     */
+    }
+    }
     
     motor1.stop(0);
     motor2.stop(0);
     
     wait(20);     //確認用
     
-     FILE *fp = fopen("/local/gps,foto.txt", "w");  // Open "gps.txt" on the local file system for writing
+//     FILE *fp = fopen("/local/gps,foto.txt", "w");  // Open "gps.txt" on the local file system for writing
     
-/* printf("GPS start\r\n");
+ printf("GPS start\r\n");
    FILE *fp = fopen("/local/gps,foto.txt", "w");  // Open "gps.txt" on the local file system for writing
     fprintf(fp, "GPS Start\r\n");
     int n;
@@ -205,7 +213,7 @@
         float X = x*0.000597964111328125;
         float Y = y*0.000597964111328125;
         float Z = z*0.000597964111328125; 
-        double a = X*X+Y*Y+Z*Z-95.982071137936;
+  /*      double a = X*X+Y*Y+Z*Z-95.982071137936;
         if (a>0){
         a = sqrt(a);
         }
@@ -225,10 +233,12 @@
 
         // 変位計算(速度を台形積分する)
         difference = ((speed + oldSpeed) * timespan) / 2 + difference;
-        oldSpeed = speed;
+        oldSpeed = speed;*/
 
         //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
-        printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示*/
+     /*   printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示*/
+     
+        printf("X=%f,Y=%f,Z=%f\r\n",X,Y,Z);
     
         printf("%d\r\n", test.read());              //フォトインタラプタ
         printf("%d\r\n", test2.read());
@@ -239,7 +249,7 @@
         else if (test.read() == 0 and flag == 1){   //そうじゃなくて今度はとうとうtestが0でスリットの部分になった瞬間なのにスイッチが1で切れているときは
             flag = 0;                               //まずこれでスイッチを0にして入れる。
                                                     //こうすることで同じスリットの中でtestが複数回0を返した時に何回も52.5mmを加算しつづけるということがなくなる
-            rightrun += 56.25;                            //総距離runに52.5を加算する
+            rightrun += 56.5625;                            //総距離runに52.5を加算する
             printf("test.read else\r\n");
         }
          if (test2.read() == 1 and flag2 == 0){
@@ -248,7 +258,7 @@
         }
         else if (test2.read() == 0 and flag2 == 1){
             flag2 = 0;
-            leftrun2 += 56.25;
+            leftrun2 += 56.5625;
             printf("test2.read else\r\n");
         }
         printf("%f", rightrun);
@@ -302,11 +312,11 @@
         wait(0.01);
         
         tt++;
-        if(tt == 10)
+        if(tt == 100)
         {
             fprintf(fp, "accel.rightrun.leftrun2\r\n");
             
-            fprintf(fp,"(%lf, %lf,%lf)\r\n", difference, rightrun, leftrun2);//加速度とフォトインタラプタによる距離を出力
+            fprintf(fp,"(%lf, %lf, %lf, %lf,%lf)\r\n", X, Y, Z, rightrun, leftrun2);//加速度とフォトインタラプタによる距離を出力
             tt=0;
         }    
         
@@ -373,14 +383,14 @@
             else if (test.read() == 0 and flag == 1)
             {
                 flag=0;
-                rightrun += 56.25;
+                rightrun += 56.5625;
             }
              if (test2.read() == 1 and flag2 == 0){
             flag2 = 1;
         }
         else if (test2.read() == 0 and flag2 == 1){
             flag2 = 0;
-            leftrun2 += 56.25;
+            leftrun2 += 56.5625;
         }
         printf("%f", rightrun);
         printf("\t%f\r\n", leftrun2);
@@ -394,7 +404,7 @@
         float X = x*0.000597964111328125;
         float Y = y*0.000597964111328125;
         float Z = z*0.000597964111328125; 
-        double a = X*X+Y*Y+Z*Z-95.982071137936;
+    /*    double a = X*X+Y*Y+Z*Z-95.982071137936;
         if (a>0){
         a = sqrt(a);
         }
@@ -417,7 +427,7 @@
         oldSpeed = speed;
 
         //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
-        printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示*/
+    //    printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示*/
         
         if (run >= 300000){                             //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
             break;                                  //つまりゴールについたらこのループからぬける
@@ -499,7 +509,7 @@
             
             
             fprintf(fp, "last accel.photo\r\n");
-            fprintf(fp,"(%lf, %lf)\r\n", difference, run);//最後の加速度とフォトインタラプタによる距離を出力
+            fprintf(fp,"(%lf)\r\n", run);//最後の加速度とフォトインタラプタによる距離を出力
             fprintf(fp, "last right.left\r\n");
             fprintf(fp,"(%lf, %lf)\r\n", rightrun, leftrun2);
             fclose(fp);