(多分)全部+フライトピン+新しい加速度
Dependencies: mbed
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); }