Diff: main.cpp

Revision:

16:4ba85a0e8767

Parent:

15:0f267807ce7c

--- a/main.cpp	Sun Mar 18 10:18:47 2018 +0000
+++ b/main.cpp	Mon Mar 19 17:08:25 2018 +0000
@@ -6,8 +6,9 @@
 
 #define UNLOCK 1
 #define LOCK 0
+#define KAIHOU 10 //開放判定開始時間（燃焼時間の間は止める）
 #define TIMER 30 //開放タイマー
-#define RATE 50//判定周期 謎の不具合により、145行目のkaihou.attach(_open,1/RATE);はコメントアウトしてるので、直接数字をいじってください
+#define RATE 50.0//判定周期 浮動小数点型で記述してください。
 #define ALT 1 //落下判断高度
  
 SDFileSystem    sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
@@ -66,7 +67,7 @@
     twe.baud(115200);
     twe.printf("Hello World!\r\n");
     
-/*    int ret;
+    int ret;
     lfp = fopen("/local/Alt.txt","r");
     if(lfp == NULL){
         goto normal;
@@ -81,7 +82,7 @@
     }
     
 normal:
-*/    
+    
     while(1){
         char c = twe.getc();
         if(_input(c) == 1){
@@ -97,9 +98,10 @@
         }
 
     }
-    
-    mpu.setAcceleroRange(0);
-    mpu.setGyroRange(0);
+yabai:
+
+    mpu.setAcceleroRange(3);
+    mpu.setGyroRange(3);
     bmp.Initialize(60,BMP180_OSS_ULTRA_LOW_POWER);
     twe.printf("I2C_initialize_ok\r\n");
     
@@ -120,6 +122,7 @@
     twe.printf("lacal_ok\r\n");
     
     mkdir("/sd/mydir", 0777);
+    
     fp = fopen("/sd/mydir/sdtest.txt", "a");
     if(fp == NULL) {
         error("Could not open file for write\n");
@@ -137,18 +140,18 @@
     
     while(fly == 1);        //フライトピン抜けるまで待機
     
-    twe.printf("L\r\n");
+    twe.printf("LAUNCH\r\n");
     i = 0;
     Alt_buff[0] = 0;
     fp = fopen("/sd/mydir/sdtest.txt", "a");
     if(fp == NULL)twe.printf("ERROR\r\n");
-yabai:
+
     timer1.start();
-//    kaihou.attach(_open,1/RATE);
-    kaihou.attach(_open,0.02);
+    kaihou.attach(_open,1.0/RATE);
     twe.printf("start!!\r\n");
-    
-    
+
+
+        
     while(1){
 
 /*        twe.printf("MAX:%f,Cnt:%d\r\n",Max_Alt, Cnt_para);
@@ -193,8 +196,16 @@
     if(i == 10){        //10回ごとに中央値計算
         if(tf_launch == true){
             Acc_now = median(c, 10);
+            Alt_now = median(Alt_buff2, 10);
+            Alt_now = Alt_now - Alt_gnd;
+            if(Alt_now > Max_Alt) Max_Alt = Alt_now;
+            t = timer1.read();
+            fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%d,%d\r\n",Alt_now,a[0],a[1],a[2],d[0],d[1],d[2],t,Cnt_para); 
             i = 0;
-            if(Acc_now < 0.5) tf_launch = false;
+            if(Acc_now < 1.0 && t > KAIHOU){
+                tf_launch = false;
+                twe.printf("accelero_ok\r\n"); 
+            }
             
         }else{
             
@@ -206,7 +217,7 @@
             i = 0;
             if(tf_para == true){    //パラ開くまでは頂点判定
                 Alt_buff[Cnt_buff+1] = Alt_now; 
-                if(Alt_buff[Cnt_buff]-Alt_buff[Cnt_buff+1] > ALT) Cnt_para++; //直前の値(0.2秒前より1ALTm降下)より小さければカウント+1
+                if(Alt_buff[Cnt_buff]-Alt_buff[Cnt_buff+1] > ALT) Cnt_para++; //直前の値(0.2秒前よりALTm降下)より小さければカウント+1
 //              twe.printf("Cnt_para:%d\r\n", Cnt_para); 
                 Cnt_buff++;                   
                 if(Cnt_para == 10 || t > TIMER){    //頂点！！！
@@ -287,11 +298,11 @@
 
 void _para(int motion){
     if(motion==UNLOCK){
-            servo_para.pulsewidth(0.0005); // pulse servo out sita
-            servo_para2.pulsewidth(0.0005);
+            servo_para.pulsewidth(0.0008); // pulse servo out sita
+            servo_para2.pulsewidth(0.0008);
     }else if(motion==LOCK){
-            servo_para.pulsewidth(0.0024); // pulse servo outu sita
-            servo_para2.pulsewidth(0.0024);     
+            servo_para.pulsewidth(0.0023); // pulse servo outu sita
+            servo_para2.pulsewidth(0.0023);     
     }
 }