Diff: main.cpp

Branch:

SDandCadenceThread

Revision:

76:2ac3d145942f

Parent:

73:2122c87cac9b

--- a/main.cpp	Fri Jun 16 14:33:37 2017 +0000
+++ b/main.cpp	Sat Jun 24 14:11:19 2017 +0000
@@ -10,8 +10,8 @@
 
 #define SOUDA_DATAS_NUM 28 //(yokutan 7 + input 7)*2
 #define YOKUTAN_DATAS_NUM 14
-#define WRITE_DATAS_NUM 34 // souda_datas_num + 6( rpy, airspeed, height, cadence)
-#define SD_WRITE_NUM 20
+#define WRITE_DATAS_NUM 25 // time + yokutan_datas_num+ 4(input) + 6( rpy, airspeed, height, cadence)
+#define SD_WRITE_NUM 40
 #define MPU_LOOP_TIME 0.01
 #define AIR_LOOP_TIME 0.01//(0.002005)
 #define WRITE_DATAS_LOOP_TIME 1
@@ -20,17 +20,23 @@
 #define MPU_DELT_MIN 250
 #define INIT_SERVO_PERIOD_MS 20
 
+#define DEBUG(...) { pc.printf(__VA_ARGS__); }
+
 //-----------------------------------(resetInterrupt def)
 extern "C" void mbed_reset();
 InterruptIn resetPin(p25);
-Timer resetTimeCount;
+//Timer resetTimeCount;
+Timer mainTimer;
 void resetInterrupt()
 {
+    int start = mainTimer.read();
     while(resetPin) {
-        resetTimeCount.start();
-        if(resetTimeCount.read()>3) mbed_reset();
+//        resetTimeCount.start();
+//        if(resetTimeCount.read()>3) mbed_reset();
+        if(mainTimer.read()-start > 3) mbed_reset();
     }
-    resetTimeCount.reset();
+//    led3 = 0;
+//    resetTimeCount.reset();
 }
 //-------------------------------------------------------
 
@@ -110,10 +116,12 @@
     air_kaitensu++;
 //    led3 = !led3;
 }
-
+int cnt = 0;
 void call_calcAirSpeed()
 {
     air.calcAirSpeed(air_kaitensu);
+//    if(!(cnt%10))pc.printf("airspeed:%f\tair_kaitensu:%d\n",airSpeed,air_kaitensu);
+//    cnt++;
     air_kaitensu = 0;
 }
 
@@ -132,9 +140,9 @@
 void sonarCalc()
 {
     sonarV = 0;
-    for(int i = 0; i<20; i++) {
+    for(int i = 0; i<10; i++) {
         sonarV += sonarPin.read();
-        wait(0.01);
+        Thread::wait(5);
     }
     sonarDist = (sonarV/20)*2064.5;// volt*3.3*1000/1.6 (電圧/距離:3.2mV/2cm)
 }
@@ -191,6 +199,7 @@
 void init()
 {
     pc.printf("(BUILD:[" __DATE__ "/" __TIME__ "])\n\r");
+    mainTimer.start();
 //--------------------------------------(resetInterrupt init)
     resetPin.rise(resetInterrupt);
     resetPin.mode(PullDown);
@@ -321,7 +330,7 @@
         drugL = soudaDatas[26]- '0';
         inputL = atof(cErebonL);
         
-        pc.printf("erebonR:%5.5f,   drugR:%d     erebonL:%5.5f   drugL:%d",inputR,drugR,inputL,drugL);
+//        pc.printf("erebonR:%5.5f,   drugR:%d     erebonL:%5.5f   drugL:%d",inputR,drugR,inputL,drugL);
 //        pc.printf("erebonR:%s,   drugR:%d     erebonL:%s   drugL:%d",cErebonR,drugR,cErebonL,drugL);
     }//if
 //    }//while(1)
@@ -367,11 +376,16 @@
 
 void WriteDatas()
 {
-    int i;
-    for(i = 0; i < SOUDA_DATAS_NUM; i++) {
+    int i=0;
+    writeDatas[write_datas_index][i++] = mainTimer.read();
+    for(int j = 0; j < YOKUTAN_DATAS_NUM; i++,j++) {
         //writeDatas[write_datas_index][i] = 0.0;
-        writeDatas[write_datas_index][i] = (float)soudaDatas[i];
+        writeDatas[write_datas_index][i] = (float)soudaDatas[j];
     }
+    writeDatas[write_datas_index][i++] = inputR;
+    writeDatas[write_datas_index][i++] = drugR;
+    writeDatas[write_datas_index][i++] = inputL;
+    writeDatas[write_datas_index][i++] = drugL;
     writeDatas[write_datas_index][i++] = pitch;
     writeDatas[write_datas_index][i++] = roll;
     writeDatas[write_datas_index][i++] = yaw;
@@ -379,11 +393,11 @@
     writeDatas[write_datas_index][i++] = sonarDist;
     writeDatas[write_datas_index][i++] = cadenceResult;//cadence_twe.cadence;
     //writeDatas[write_datas_index][i++] = writeTimer.read();
-    //for(i = 0; i < WRITE_DATAS_NUM; i++){
-//        ////pc.printf("%f   ", writeDatas[write_datas_index][i]);
+    for(i = 0; i < WRITE_DATAS_NUM; i++){
+        DEBUG("%f ", writeDatas[write_datas_index][i]);
 //        twe.printf("%f,", writeDatas[write_datas_index][i]);
-//    }
-//    //pc.printf("\n\r");
+    }
+    DEBUG("\n\r");
 //    twe.printf("\n\r");
     if(write_datas_index == SD_WRITE_NUM-1) {
 //        SDprintf();
@@ -391,7 +405,7 @@
     } else {
         write_datas_index++;
     }
-    char sbuf[128];
+    char sbuf[YOKUTAN_DATAS_NUM*2 + 5];
     int p=0;
 //    twe.printf("con,");
     p += sprintf(sbuf,"con,");
@@ -429,7 +443,7 @@
 //              soudaDatas[YOKUTAN_DATAS_NUM],soudaDatas[sizeof(int) + YOKUTAN_DATAS_NUM + 2],(int)soudaDatas[SOUDA_DATAS_NUM - sizeof(int) - 3],soudaDatas[SOUDA_DATAS_NUM-1],
 //              pitch,roll,yaw,
 //              airSpeed,sonarDist,cadenceResult);
-    pc.printf("cadence:%5.5f\n\r",cadenceResult);
+//    pc.printf("cadence:%5.5f\n\r",cadenceResult);
 
 //    for(int i = 0; i < strlen(cadence_twe.myBuff); i++){
 //        ////pc.printf("%c",*(cadence_twe.myBuff+i));
@@ -438,21 +452,11 @@
     if(android.writeable()) {
 //        android.printf("%f,%f,%f,",pitch,roll,yaw);
 //        android.printf("%f,%f,\r\n",airSpeed,sonarDist);
-        android.printf("%4.2f,%4.2f,%4.2f,\n,",roll,airSpeed,cadenceResult);//cadence_twe.cadence);
-//        led2 = !led2;
+        android.printf("%4.2f,%4.2f,%4.2f,\n,",roll,airSpeed,cadenceResult);
     }
 //    SDprintf();
 }
 
-void WriteDatasF()
-{
-    //pc.printf("airSpeed:%f\n\r",airSpeed);
-}
-
-//float calcKXdeg(float x){
-//    return -310.54*x+156.65;
-//}
-
 void RollAlarm()
 {
     if((roll < -ROLL_L_MAX_DEG ) && (roll > ROLL_L_MAX_DEG-180)) {
@@ -474,14 +478,14 @@
     Thread SD_thread(&SDprintf);
 //    Thread soudaSerial_thread(&DataReceiveFromSouda);
     init();
-    int VCcounter = 0;
+//    int VCcounter = 0;
     while(1) {
-        if(VCcounter%20 == 0 ) {
-            if( VCmonitor.getVoltage(&V) == 0) {
-                pc.printf("e:%f\n",V);
-            }
-        }
-        VCcounter++;
+//        if(VCcounter%20 == 0 ) {
+//            if( VCmonitor.getVoltage(&V) == 0) {
+//                pc.printf("e:%f\n",V);
+//            }
+//        }
+//        VCcounter++;
 
 //        updateCadence(V,mgPin.read() * 3.3,mgPin2.read() * 3.3,isFirstCadenceFlag);
         //pc.printf("test\n\r");