Diff: main.cpp

Branch:

SDandCadenceThread

Revision:

75:85a5e39b2f3c

Parent:

73:2122c87cac9b

--- a/main.cpp	Fri Jun 16 14:33:37 2017 +0000
+++ b/main.cpp	Sat Jun 24 06:47:41 2017 +0000
@@ -5,13 +5,15 @@
 #include "Fusokukei.h"
 #include "MPU6050.h"
 #include "BufferedSoftSerial.h"
-#include "SDFileSystem.h"//2014.6/5以前の環境で動作します。アップデートすると動きません。
+//#include "SDFileSystem.h"//2014.6/5以前の環境で動作します。アップデートすると動きません。
 #include "INA226.hpp"
 
+#define CadenceDebug(...) pc.printf(__VA_ARGS__)
+
 #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 35 // timer + souda_datas_num + 6( rpy, airspeed, height, cadence)// time 追加
+#define SD_WRITE_NUM 20 
 #define MPU_LOOP_TIME 0.01
 #define AIR_LOOP_TIME 0.01//(0.002005)
 #define WRITE_DATAS_LOOP_TIME 1
@@ -20,17 +22,21 @@
 #define MPU_DELT_MIN 250
 #define INIT_SERVO_PERIOD_MS 20
 
+Timer mbedTimer;
+
 //-----------------------------------(resetInterrupt def)
 extern "C" void mbed_reset();
 InterruptIn resetPin(p25);
-Timer resetTimeCount;
+//Timer resetTimeCount;
 void resetInterrupt()
 {
+    int start = mbedTimer.read();
     while(resetPin) {
-        resetTimeCount.start();
-        if(resetTimeCount.read()>3) mbed_reset();
+//        resetTimeCount.start();
+//        if(resetTimeCount.read()>3) mbed_reset();
+        if(mbedTimer.read() - start >3) mbed_reset();
     }
-    resetTimeCount.reset();
+//    resetTimeCount.reset();
 }
 //-------------------------------------------------------
 
@@ -83,6 +89,7 @@
 volatile int write_datas_index = 0;
 float inputR,inputL;
 int drugR,drugL;
+//Timer dataTimer;
 
 void air_countUp();
 void call_calcAirSpeed();
@@ -95,7 +102,7 @@
 void mpuProcessing(void const *arg);
 void DataReceiveFromSouda(void const *arg);
 void SdInit();
-void SDprintf();
+void Logprint();
 void WriteDatas();
 float calcAttackAngle();
 float calcKXdeg(float x);
@@ -132,11 +139,11 @@
 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)
+    sonarDist = (sonarV/10)*2064.5;// volt*3.3*1000/1.6 (電圧/距離:3.2mV/2cm)
 }
 
 
@@ -191,6 +198,8 @@
 void init()
 {
     pc.printf("(BUILD:[" __DATE__ "/" __TIME__ "])\n\r");
+    
+    mbedTimer.start();
 //--------------------------------------(resetInterrupt init)
     resetPin.rise(resetInterrupt);
     resetPin.mode(PullDown);
@@ -210,7 +219,7 @@
     unsigned short val;
     val = 0;
     if(VCmonitor.rawRead(0x00,&val) != 0) {
-        printf("VCmonitor READ ERROR\n");
+        pc.printf("VCmonitor READ ERROR\n");
         while(1) {}
     }
     VCmonitor.setCurrentCalibration();
@@ -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)
@@ -339,12 +348,12 @@
     fclose(fp);
 }
 
-void SDprintf(const void* arg)
+void Logprint(const void* arg)
 {
     SdInit();
     while(1) {
         updateCadence(V,mgPin.read() * 3300.0,mgPin2.read() * 3300.0);
-        //  pc.printf("V:%5.5f  mgPin:%5.5f     mgPin2:%5.5f",V,mgPin.read() * 3300.0,mgPin2.read() * 3300.0);
+        CadenceDebug("V:%f  mgPin:%5.5f     mgPin2:%5.5f     cadence:%f\n\r",V,mgPin.read() * 3300.0,mgPin2.read() * 3300.0,cadenceResult);
         if(write_datas_index == SD_WRITE_NUM-1) {
             fp = fopen("/local/data.csv", "a");
             if(fp == NULL) {
@@ -367,8 +376,9 @@
 
 void WriteDatas()
 {
-    int i;
-    for(i = 0; i < SOUDA_DATAS_NUM; i++) {
+    int i = 0;
+    writeDatas[write_datas_index][i] = mbedTimer.read();
+    for(; i <= SOUDA_DATAS_NUM; i++) {
         //writeDatas[write_datas_index][i] = 0.0;
         writeDatas[write_datas_index][i] = (float)soudaDatas[i];
     }
@@ -386,11 +396,12 @@
 //    //pc.printf("\n\r");
 //    twe.printf("\n\r");
     if(write_datas_index == SD_WRITE_NUM-1) {
-//        SDprintf();
+//        Logprint();
         write_datas_index=0;
     } else {
         write_datas_index++;
     }
+    
     char sbuf[128];
     int p=0;
 //    twe.printf("con,");
@@ -405,6 +416,7 @@
             p += sprintf(sbuf+p,"%d\n",soudaDatas[i]);
     }
     twe.printf("%s",sbuf);
+    
 //    twe.printf("inp,%f,%i,%f,%i\n",soudaDatas[YOKUTAN_DATAS_NUM],soudaDatas[sizeof(float) + YOKUTAN_DATAS_NUM + 2],(int)soudaDatas[SOUDA_DATAS_NUM - sizeof(float) - 3],soudaDatas[SOUDA_DATAS_NUM-1]);
     twe.printf("inp,%f,%i,%f,%i\n",inputR,drugR,inputL,drugL);
 
@@ -420,16 +432,14 @@
     twe.printf("mpu,%f,%f,%f\n",pitch,roll,yaw);
     twe.printf("kei,%f,%f,%f\n",airSpeed,sonarDist,cadenceResult);//cadence_twe.cadence);
 
-    ////pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
+//    pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
 //    pc.printf("%f,%f,%f\n\r",airSpeed,sonarDist,cadenceResult);//cadence_twe.cadence);
 //    pc.printf("%d,%i,%d,%i,",soudaDatas[YOKUTAN_DATAS_NUM],soudaDatas[sizeof(int) + YOKUTAN_DATAS_NUM + 2],(int)soudaDatas[SOUDA_DATAS_NUM - sizeof(int) - 3],soudaDatas[SOUDA_DATAS_NUM-1]);
 //    pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
-//    printf("mgPin V:%f\n\r",mgPin.read()*3.3);
-    // pc.printf("%d,%i,%d,%i\n%f,%f,%f\n%f,%f,%f\n\r",
+//    pc.printf("%d,%i,%d,%i\n%f,%f,%f\n%f,%f,%f\n\r",
 //              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);
 
 //    for(int i = 0; i < strlen(cadence_twe.myBuff); i++){
 //        ////pc.printf("%c",*(cadence_twe.myBuff+i));
@@ -441,7 +451,7 @@
         android.printf("%4.2f,%4.2f,%4.2f,\n,",roll,airSpeed,cadenceResult);//cadence_twe.cadence);
 //        led2 = !led2;
     }
-//    SDprintf();
+//    Logprint();
 }
 
 void WriteDatasF()
@@ -471,17 +481,17 @@
 int main()
 {
     Thread mpu_thread(&mpuProcessing);
-    Thread SD_thread(&SDprintf);
+    Thread Log_thread(&Logprint);
 //    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");