Diff: main.cpp

Branch:

cadence

Revision:

61:7f980cb3a7a8

Parent:

59:7cb8eaf553ef

Child:

62:98294011f568

--- a/main.cpp	Sat Jun 10 05:55:44 2017 +0000
+++ b/main.cpp	Tue Jun 13 06:11:51 2017 +0000
@@ -6,6 +6,7 @@
 #include "MPU6050.h"
 #include "BufferedSoftSerial.h"
 #include "SDFileSystem.h"//2014.6/5以前の環境で動作します。アップデートすると動きません。
+#include "INA226.hpp"
 
 #define SOUDA_DATAS_NUM 28 //(yokutan 7 + input 7)*2
 #define YOKUTAN_DATAS_NUM 14
@@ -62,6 +63,8 @@
 uint32_t sumCount = 0;
 MPU6050 mpu6050;
 Timer t;
+Timer cadenceTimer;
+
 //Ticker mpu6050Ticker;
 
 DigitalOut RollAlarmR(p23);
@@ -69,6 +72,10 @@
 DigitalOut led2(LED2);
 //DigitalOut led3(LED3);
 DigitalOut led4(LED4);
+I2C i2c(p9,p10);
+INA226 VCmonitor(i2c,0x9C);
+AnalogIn mgPin(p20);
+AnalogIn mgPin2(p19);
 
 char soudaDatas[SOUDA_DATAS_NUM];
 float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
@@ -78,7 +85,7 @@
 void call_calcAirSpeed();
 void sonarInterruptStart();
 void sonarInterruptStop();
-void updateCadence(/*void const *arg*/);
+void updateCadence(double source, double input,double input2,bool isFFlag);
 void init();
 void FusokukeiInit();
 void MpuInit();
@@ -89,6 +96,11 @@
 void WriteDatas();
 float calcAttackAngle();
 float calcKXdeg(float x);
+int lastCadenceInput = 0;           //1つ前のケイデンスのパルス値を取得します。これの取りうる値は0か1です。
+int lastCadenceInput2 = 0;           //1つ前のケイデンスのパルス値を取得します。これの取りうる値は0か1です。
+double cadenceResult = 0.0;         //最終的なケイデンスの値です。
+int cadenceCounter = 0;             //クランクが一回転すると、二つのセンサがそれぞれ2回ずつ状態が変化するため、0～4をカウントするためのカウンタです。
+double V;
 
 void air_countUp()
 {
@@ -144,12 +156,25 @@
     return (a * source + b < input) ? 1 : 0;
 }
 
-void updateCadence(/*void const *arg*/)
+//ケイデンスの値を取得します。
+// source: 定格12V電源の電圧値[mV], input: センサ値[mV]
+void updateCadence(double source, double input,double input2,bool isFFlag)
 {
-//    while(1){
-    cadence_twe.readData();
-//        Thread::wait(5);
-//    }
+    if(isFFlag) {
+        lastCadenceInput =  isOh182eOverThreshold(source,input);
+        lastCadenceInput2 =  isOh182eOverThreshold(source,input2);
+        cadenceTimer.start();
+        return;
+    }
+    if((isOh182eOverThreshold(source,input) ^ lastCadenceInput) ||(isOh182eOverThreshold(source,input2) ^ lastCadenceInput2)) {
+        if(cadenceCounter < 4) {
+            cadenceCounter++;
+            return;
+        }
+        cadenceResult =60.0/  (cadenceTimer.read_us() / 1000000.0); //クランク一回転にかかる時間を取得
+        cadenceTimer.reset();
+        cadenceCounter = 0;
+    }
 }
 
 void init()
@@ -166,12 +191,18 @@
 //    SdInit();
 //    MpuInit();
     //writeDatasTicker.attach(&WriteDatas,1);
-//    cadenceUpdateTicker.attach(&updateCadence, 0.2);
 
 //-----for InterruptMode of sonar----------------------------
 //    sonarPin.rise(&sonarInterruptStart);
 //    sonarPin.fall(&sonarInterruptStop);
 //-----------------------------------------------------------
+    unsigned short val;
+    val = 0;
+    if(VCmonitor.rawRead(0x00,&val) != 0) {
+        printf("VCmonitor READ ERROR\n");
+        while(1) {}
+    }
+    VCmonitor.setCurrentCalibration();
 }
 
 void FusokukeiInit()
@@ -204,7 +235,6 @@
 double calcPulse(int deg)
 {
     return (0.0006+(deg/180.0)*(0.00235-0.00045));
-
 }
 
 void mpuProcessing(void const *arg)
@@ -268,7 +298,8 @@
 //    }//while(1)
 }
 
-void SdInit(){
+void SdInit()
+{
     mkdir("/sd/mydir", 0777);
     fp = fopen("/sd/mydir/sdtest2.csv", "w");
     if(fp == NULL) {
@@ -278,22 +309,23 @@
     fclose(fp);
 }
 
-void SDprintf(const void* arg){
+void SDprintf(const void* arg)
+{
     SdInit();
-    while(1){
-        if(write_datas_index == SD_WRITE_NUM-1){
+    while(1) {
+        if(write_datas_index == SD_WRITE_NUM-1) {
             fp = fopen("/sd/mydir/data.csv", "a");
             if(fp == NULL) {
                 printf("Could not open file for write\n");
             }
-            for(int i = 0; i < SD_WRITE_NUM; i++){
-                for(int j = 0; j < WRITE_DATAS_NUM; j++){
+            for(int i = 0; i < SD_WRITE_NUM; i++) {
+                for(int j = 0; j < WRITE_DATAS_NUM; j++) {
                     fprintf(fp,"%f,", writeDatas[i][j]);
                 }
             }
             fprintf(fp,"\n");
             fclose(fp);
-            
+
             write_datas_index=0;
         }
         Thread::wait(1);
@@ -355,9 +387,6 @@
 //    }
 //    pc.printf("%f\t%f\t%f\t%f\n\r",airSpeed,air_sum[0],air_sum[1],air_sum[2]);
     if(android.writeable()) {
-//        for(int i = 0; i<SOUDA_DATAS_NUM; i++){
-//            android.printf("%i,",soudaDatas[i]);
-//        }
 //        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,cadence_twe.cadence);
@@ -375,10 +404,6 @@
 //    return -310.54*x+156.65;
 //}
 
-//float calcAttackAngle(){
-//    return pitch-calcKXdeg(kx_Z.read());
-//}
-
 void RollAlarm()
 {
     if((roll < -ROLL_L_MAX_DEG ) && (roll > ROLL_L_MAX_DEG-180)) {
@@ -394,37 +419,27 @@
     }
 }
 
-void WriteServo()
-{
-    //kisokuServo.pulsewidth(calcPulse(airSpeed*10));
-//    kisokuServo.pulsewidth(calcPulse(9*airSpeed));
-    if(pitch<0) {
-//        geikakuServo.pulsewidth(calcPulse(0));
-    } else {
-//        geikakuServo.pulsewidth(calcPulse(abs(pitch*90/13.0)));
-    }
-    //////////pc.printf("a:%f",airSpeed);
-    //pc.printf("p:%f\r\n",pitch);
-    //kisokuServo.pulsewidth(calcPulse(0));
-    //geikakuServo.pulsewidth(calcPulse(0));
-}
-
 int main()
 {
-    Thread cadence_thread(&updateCadence);
     Thread mpu_thread(&mpuProcessing);
     Thread SD_thread(&SDprintf);
+    bool isFirstCadenceFlag = true;
 //    Thread soudaSerial_thread(&DataReceiveFromSouda);
     init();
+
     while(1) {
+        if(VCmonitor.getVoltage(&V) == 0) {
+            printf("e:%f\n",V);
+        }
+        printf("mgPin V:%f\n\r",mgPin.read()*3.3);
+        updateCadence(V,mgPin.read() * 3.3,mgPin2.read() * 3.3,isFirstCadenceFlag);
+        isFirstCadenceFlag = false;
         //pc.printf("test\n\r");
 //        mpuProcessing();
         sonarCalc();
         RollAlarm();
         DataReceiveFromSouda();
-//        updateCadence();
         WriteDatas();
-//        WriteServo();
         led4 = !led4;
     }
 }
\ No newline at end of file