(多分)全部+フライトピン+新しい加速度
Dependencies: mbed
Diff: main.cpp
- Revision:
- 26:ee62ea2f9362
- Parent:
- 25:6680d67bdf24
--- a/main.cpp Sat Dec 08 02:46:08 2018 +0000 +++ b/main.cpp Sat Dec 08 07:27:12 2018 +0000 @@ -104,7 +104,7 @@ //地磁気センサのキャリブレーション motor1.speed(mc1); //車体を時計回りに3秒回転 motor2.speed(-mc2); - wait(3); + wait(1.6); motor1.stop(0); motor2.stop(0); @@ -112,7 +112,7 @@ motor1.speed(-mc1); //車体を反時計回りに3秒回転 motor2.speed(mc2); - wait(3); + wait(1.6); motor1.stop(0); motor2.stop(0); @@ -125,24 +125,24 @@ mcn2=1.0; compass.init(); heading=compass.getHeadingXYDeg(); -if(92.5<heading<=270.0){ - motor1.speed(mcn1); +if(90.0<heading<267.5){ + motor1.speed(mcn1);//右回転 motor2.speed(-mcn2); - wait((heading-90.0)*0.004722); //角度のずれ*1度回転するのにかかる時間 + wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間 motor1.stop(0); motor2.stop(0); wait(1); -}else if(270.0<heading<PI2){ - motor1.speed(-mcn1); +}else if(0.0<=heading<=90.0){ + motor1.speed(-mcn1);//左回転 motor2.speed(mcn2); - wait((PI2-heading+90.0)*0.004722); + wait((heading+90.0)*0.004448); motor1.stop(0); motor2.stop(0); wait(1); -}else if(0<=heading<87.5){ - motor1.speed(-mcn1); +}else if(272.5<heading<=360){ + motor1.speed(-mcn1);//左回転 motor2.speed(mcn2); - wait((90.0-heading)*0.004722); + wait((heading-270)*0.004448); motor1.stop(0); motor2.stop(0); wait(1); @@ -151,6 +151,7 @@ } printf("searchN\r\n"); //機体が北を向く + mu.startUpdates();//start mesuring the distance(超音波センサー) int distance; @@ -268,7 +269,7 @@ motor1.speed(msj1); //機体を時計回りに90度回転 motor2.speed(-msj2); - wait(0.77); + wait(0.4); printf("mortor rotation\r\n"); motor1.stop(0); @@ -288,7 +289,7 @@ motor1.speed(-msj1); //機体を反時計回りに90度回転 motor2.speed(msj2); - wait(0.77); + wait(0.4); printf("mortor rotation\r\n"); motor1.stop(0); @@ -372,7 +373,7 @@ motor1.speed(-msj1); //機体を反時計回りに90度回転 motor2.speed(msj2); - wait(1); + wait(0.4); printf("mortor rotation\r\n"); motor1.stop(0); @@ -387,7 +388,7 @@ motor1.stop(0); motor2.stop(0); - wait(2); + wait(0.4); printf("mortor stop\r\n"); motor1.speed(msj1); //機体を時計回りに90度回転 @@ -400,6 +401,31 @@ wait(2); printf("mortor stop\r\n"); +if(90.0<heading<267.5){ + motor1.speed(mcn1);//右回転 + motor2.speed(-mcn2); + wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間 + motor1.stop(0); + motor2.stop(0); + wait(1); +}else if(0.0<=heading<=90.0){ + motor1.speed(-mcn1);//左回転 + motor2.speed(mcn2); + wait((heading+90.0)*0.004448); + motor1.stop(0); + motor2.stop(0); + wait(1); +}else if(272.5<heading<=360){ + motor1.speed(-mcn1);//左回転 + motor2.speed(mcn2); + wait((heading-270)*0.004448); + motor1.stop(0); + motor2.stop(0); + wait(1); +}else{ + wait(5); +} +printf("searchN\r\n"); //機体が北を向く } }