(多分)全部+フライトピン+新しい加速度
Dependencies: mbed
Diff: main.cpp
- Revision:
- 10:280a25bcc8bb
- Parent:
- 9:fb19a93df88f
- Child:
- 11:80284e989390
--- a/main.cpp Fri Oct 26 07:04:55 2018 +0000 +++ b/main.cpp Mon Nov 05 12:46:14 2018 +0000 @@ -10,7 +10,7 @@ GPS gps(p28, p27); //GPS HMC5883L compass(p9, p10); //地磁気センサー -MPU6050 mpu(p9,p10); //(SDA,SCL)のピンをアサインしてね☆ 加速度センサー +MPU6050 mpu(p9, p10); //(SDA,SCL)のピンをアサインしてね☆ 加速度センサー DigitalOut FET(p21); //FET @@ -75,14 +75,56 @@ - compass.init(); //地磁気センサー動作 +compass.init(); //地磁気センサー動作 - int i; - for(i=0;i<20;i++) //地磁気測定 - { - pc.printf("raw=%f\r\n",compass.getHeadingXYDeg()); //度数法で表記 - wait(0.5); - } +int i; +for(i=0;i<20;i++) //地磁気測定 +{ + pc.printf("raw=%f\r\n",compass.getHeadingXYDeg()); //度数法で表記 + wait(0.5); +} +float mc1,mc2; + mc1=3.0; + mc2=3.0; + //地磁気センサのキャリブレーション + motor1.speed(mc1); //車体を時計回りに3秒回転 + motor2.speed(-mc2); + wait(2); + + motor1.stop(0); + motor2.stop(0); + wait(1); + + motor1.speed(-mc1); //車体を反時計回りに3秒回転 + motor2.speed(mc2); + wait(2); + + motor1.stop(0); + motor2.stop(0); + wait(1); + printf("compass carriblation\r\n"); //キャリブレーション終了 + +float mcr1,mcr2,mcl1,mcl2; +double heading; + mcr1=heading*10*0.00026; + mcr2=heading*10*0.00026; + mcl1=(PI2-heading)*10*0.00026; + mcl2=(PI2-heading)*10*0.00026; +compass.init(); +if(2.5<heading<=M_PI){ + motor1.speed(mcr1); + motor2.speed(-mcr2); + wait(1); +}else if(M_PI<heading<357.5){ + motor1.speed(-mcl1); + motor2.speed(mcl2); + wait(1); +}else{ + wait(2); +} +printf("searchN\r\n"); //機体が北を向く + + mu.startUpdates();//start mesuring the distance(超音波センサー) int distance; @@ -250,6 +292,8 @@ motor2.stop(0); wait(2); printf("mortor stop\r\n"); + + } }