Diff: main.cpp

Revision:

17:9bc130ebb5ed

Parent:

16:44c763c32b0d

Child:

18:2a47ed430cfe

--- a/main.cpp	Thu Nov 15 09:18:24 2018 +0000
+++ b/main.cpp	Tue Nov 20 06:43:02 2018 +0000
@@ -12,7 +12,7 @@
 
 MPU6050 mpu(p9, p10);                //(SDA,SCL)のピンをアサインしてね☆   加速度センサー
 
-
+DigitalIn flight(p22);      //フライトピン
 DigitalOut FET(p21);                //FET
 
 InterruptIn button1(p15);           //フォトインタラプタ
@@ -41,12 +41,25 @@
  
 int main() {
     
-    
-      /*  FET = 0;                       //FET、ニクロム線切断
-        wait(60);
+     FET = 0; 
+     while(1) {
+    if(flight==1) {
+        wait(10);
+        }
+        
+    else{
+    FET = 0;                       //FET、ニクロム線切断
+        wait(20);
         FET = 1;
-        wait(30);
-        FET = 0;   */
+        wait(12);
+        FET = 0;
+        wait(10);
+        FET = 1;
+        wait(12);
+        FET = 0; 
+    break;
+            }
+    }
     
     motor1.stop(0);
     motor2.stop(0);
@@ -137,15 +150,15 @@
         rightrun=0.0;
         leftrun2=0.0;
         
-        float filterCoefficient = 0.3; // ローパスフィルターの係数(これは環境によって要調整。1に近づけるほど平滑化の度合いが大きくなる。
+        float filterCoefficient = 0.9; // ローパスフィルターの係数(これは環境によって要調整。1に近づけるほど平滑化の度合いが大きくなる。
         float lowpassValue = 0;
         float highpassValue = 0;
         float speed = 0;//加速度時から算出した速度
         float oldSpeed = 0;//ひとつ前の速度
         float oldAccel = 0;//ひとつ前の加速度
         float difference=0;//変位
-        float timespan=0.1;//時間差
-        float accel[3];//accelを3つの配列で定義。         加速度センサー*/
+        float timespan=0.01;//時間差
+        float accel[3];//accelを3つの配列で定義。
 
         while(1)
     {       
@@ -154,23 +167,24 @@
         
         mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
         float x = accel[0]/16384-0.043;//x軸方向の加速度
-        float y = accel[1]/16384-0.012;//y軸方向の加速度
-        float z = accel[2]/16384;//z軸方向の加速度
-        printf("accel x:%f y:%f z:%f\r\n",x,y,z);//一応表示しとくやで～～
+        float y = accel[0]/16384;//y軸方向の加速度
+        float z = accel[0]/16384;//z軸方向の加速度
         // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (1 - 係数) * センサの値)
         lowpassValue = lowpassValue * filterCoefficient + x * (1 - filterCoefficient);
-        // ハイパスフィルター(センサの値 - ローパスフィルターの値)
+        // ハイパスフィルター(センサの値 - ローパスフィルターの値)//
         highpassValue = x - lowpassValue;
-        
+
         // 速度計算(加速度を台形積分する)
         speed = ((highpassValue + oldAccel) * timespan) / 2 + speed;
         oldAccel = highpassValue;
-        
+
         // 変位計算(速度を台形積分する)
         difference = ((speed + oldSpeed) * timespan) / 2 + difference;
         oldSpeed = speed;
-        
-        printf(" speed %f difference %f\n",speed,difference);
+
+        //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
+        //printf("%f,",speed);//速度を表示
+        printf("%f,",difference);//変位を表示
     
         printf("%d\r\n", test.read());              //フォトインタラプタ
         printf("%d\r\n", test2.read());
@@ -286,23 +300,26 @@
         
         mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
         float x = accel[0]/16384-0.043;//x軸方向の加速度
-        float y = accel[1]/16384-0.012;//y軸方向の加速度
-        float z = accel[2]/16384;//z軸方向の加速度
-        printf("accel x:%f y:%f z:%f\r\n",x,y,z);//一応表示しとくやで～～
+        float y = accel[0]/16384;//y軸方向の加速度
+        float z = accel[0]/16384;//z軸方向の加速度
         // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (1 - 係数) * センサの値)
         lowpassValue = lowpassValue * filterCoefficient + x * (1 - filterCoefficient);
-        // ハイパスフィルター(センサの値 - ローパスフィルターの値)
+        // ハイパスフィルター(センサの値 - ローパスフィルターの値)//
         highpassValue = x - lowpassValue;
-        
+
         // 速度計算(加速度を台形積分する)
         speed = ((highpassValue + oldAccel) * timespan) / 2 + speed;
         oldAccel = highpassValue;
-        
+
         // 変位計算(速度を台形積分する)
         difference = ((speed + oldSpeed) * timespan) / 2 + difference;
         oldSpeed = speed;
+
+        //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
+        //printf("%f,",speed);//速度を表示
+        printf("%f,",difference);//変位を表示
         
-        printf(" speed %f difference %f\n",speed,difference);
+        wait(0.01);
     }