(多分)全部+フライトピン+新しい加速度
Dependencies: mbed
Diff: main.cpp
- Revision:
- 20:e78bffff80ee
- Parent:
- 19:8ad7dcfef11f
- Child:
- 22:1e01f4f7097c
- Child:
- 23:2ca79873ef44
diff -r 8ad7dcfef11f -r e78bffff80ee main.cpp --- a/main.cpp Sun Nov 25 04:12:06 2018 +0000 +++ b/main.cpp Tue Nov 27 01:11:38 2018 +0000 @@ -41,14 +41,14 @@ int main() { - /* FET = 0; + FET = 0; while(1) { if(flight==1) { wait(10); } else{ - FET = 0; //FET、ニクロム線切断 + FET = 0; //FET、ニクロム線切断 wait(20); FET = 1; wait(12); @@ -59,7 +59,7 @@ FET = 0; break; } - }*/ + } motor1.stop(0); motor2.stop(0); @@ -99,12 +99,12 @@ wait(0.5); } float mc1,mc2; - mc1=3.0; - mc2=3.0; + mc1=1.0; + mc2=1.0; //地磁気センサのキャリブレーション motor1.speed(mc1); //車体を時計回りに3秒回転 motor2.speed(-mc2); - wait(2); + wait(3); motor1.stop(0); motor2.stop(0); @@ -112,34 +112,37 @@ motor1.speed(-mc1); //車体を反時計回りに3秒回転 motor2.speed(mc2); - wait(2); + wait(3); motor1.stop(0); motor2.stop(0); wait(1); printf("compass carriblation\r\n"); //キャリブレーション終了 -float mcr1,mcr2,mcl1,mcl2; +float mcn1,mcn2; double heading; - mcr1=heading*10*0.00026; - mcr2=heading*10*0.00026; - mcl1=(PI2-heading)*10*0.00026; - mcl2=(PI2-heading)*10*0.00026; + mcn1=1.0; + mcn2=1.0; compass.init(); +heading=compass.getHeadingXYDeg(); if(2.5<heading<=M_PI){ - motor1.speed(mcr1); - motor2.speed(-mcr2); + motor1.speed(mcn1); + motor2.speed(-mcn2); + wait(heading*0.01); //角度のずれ*1度回転するのにかかる時間 + motor1.stop(0); + motor2.stop(0); wait(1); }else if(M_PI<heading<357.5){ - motor1.speed(-mcl1); - motor2.speed(mcl2); + motor1.speed(-mcn1); + motor2.speed(mcn2); + wait((PI2-heading)*0.01); + motor1.stop(0); + motor2.stop(0); wait(1); }else{ - wait(2); + wait(5); } printf("searchN\r\n"); //機体が北を向く - - mu.startUpdates();//start mesuring the distance(超音波センサー) int distance;