Diff: main.cpp

Revision:

30:66fa18093418

Parent:

29:2da9b8d03c0b

Child:

32:fe006d07c1ea

--- a/main.cpp	Fri Feb 17 07:38:36 2017 +0000
+++ b/main.cpp	Fri Feb 17 09:04:48 2017 +0000
@@ -43,14 +43,12 @@
 Cadence cadence(p13,p14);
 
 RawSerial pc(USBTX,USBRX);
-//RawSerial Android(p13,p14);
 BufferedSoftSerial twe(p11,p12);
 RawSerial Android(p9,p10);
 BufferedSoftSerial soudaSerial(p17,p18);
-//Ticker writeDatasTicker;
 Timer writeTimer;
 
-InterruptIn FusokukeiPin(p22);
+InterruptIn FusokukeiPin(p23);
 Ticker FusokukeiTicker;
 Fusokukei air;
 volatile int air_kaitensu= 0;
@@ -92,22 +90,23 @@
 float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
 volatile int write_datas_index = 0;
 
-void cadenceDataReceive();
+void cadenceDataReceive(void const *argument);
 void air_countUp();
 void call_calcAirSpeed();
 void init();
 void FusokukeiInit();
 void SdInit();
-void MpuInit();
+//void MpuInit();
 //void SonarInit();
-void mpuProcessing();
-void DataReceiveFromSouda();
+void mpuProcessing(void const *argument);
+void sonarCalc(void const *argument);
+void DataReceiveFromSouda(void const *argument);
 void SDprintf(void const *argument);
 void WriteDatas();
 //float calcAttackAngle();
 //float calcKXdeg(float x);
 
-void cadenceDataReceive(){
+void cadenceDataReceive(void const *argument){
     while(1){
         cadence.readData();
         Thread::wait(0.01);
@@ -170,7 +169,7 @@
     fclose(fp);
 }
 
-void MpuInit(){
+//void MpuInit(){
 //    i2c.frequency(400000);  // use fast (400 kHz) I2C
 //    t.start();
 //    uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
@@ -189,34 +188,20 @@
 //        pc.printf("MPU6050 not ready...\n\r");
 //        //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
 //    }
-    using namespace MPU6050DMP6;
-    setup();
-    do{
-        for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i];
-        getYPR();
-        Thread::wait(0.01);
-    }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) );
-    for(int i = 0; i<3; i++) offset_ypr[i] = ypr[i];
-}
+//    using namespace MPU6050DMP6;
+//    setup();
+//    do{
+//        for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i];
+//        getYPR();
+//        Thread::wait(0.01);
+//    }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) );
+//    for(int i = 0; i<3; i++) offset_ypr[i] = ypr[i];
+//}
 
 //void SonarInit(){
 //    sonarPin.rise(&sonarInterruptStart);
 //    sonarPin.fall(&sonarInterruptStop);
 //}
-void sonarCalc(void const *argument){
-//    return sonarDistTime*0.018624 - 13.511;
-    while(1){
-        sonarV = 0;
-        for(int i =0; i<20; i++){
-            sonarV += sonarPin.read();
-            Thread::wait(0.01);
-//            wait(0.01);
-        }
-        sonarDist = (sonarV/20)*2062.5;
-        Thread::wait(0.01);
-//        wait(0.01);
-    }
-}
 
 //void mpuProcessing(void const *argument){
 //    MpuInit();
@@ -264,11 +249,11 @@
 void mpuProcessing(void const *argument){
     using namespace MPU6050DMP6;
     setup();
-    do{
-        for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i];
+//    do{
+//        for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i];
         getYPR();
-        Thread::wait(0.01);
-    }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) );
+//        Thread::wait(0.05);
+//    }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) );
     for(int i = 0; i<3; i++) offset_ypr[i] = ypr[i];
 //    MpuInit();
     while(1){
@@ -276,7 +261,22 @@
         yaw = (ypr[0] - offset_ypr[0]) *180/M_PI;
         pitch = (ypr[1] - offset_ypr[1]) *180/M_PI;
         roll = (ypr[2] - offset_ypr[2]) *180/M_PI;
+        Thread::wait(0.1);
+    }
+}
+
+void sonarCalc(void const *argument){
+//    return sonarDistTime*0.018624 - 13.511;
+    while(1){
+        sonarV = 0;
+        for(int i =0; i<20; i++){
+            sonarV += sonarPin.read();
+            Thread::wait(0.01);
+//            wait(0.01);
+        }
+        sonarDist = (sonarV/20)*2062.5;
         Thread::wait(0.01);
+//        wait(0.01);
     }
 }