Revision 67:9d2eb6793464, committed 2017-06-15

Comitter:

YusukeWakuta

Date:

Thu Jun 15 08:33:31 2017 +0000

Branch:

cadence

Parent:

66:1ffafa1c4a87

Parent:

65:075118165355

Child:

68:ed8ffcb8c49a

Commit message:

merge

Changed in this revision

--- a/main.cpp	Thu Jun 15 08:30:38 2017 +0000
+++ b/main.cpp	Thu Jun 15 08:33:31 2017 +0000
@@ -358,18 +358,20 @@
     } else {
         write_datas_index++;
     }
-//    char sbuf[128];
-//    int p=0;
-    twe.printf("con,");
-//    p = sprintf(sbuf,"con,");
+
+    char sbuf[128];
+    int p=0;
+//    twe.printf("con,");
+    p += sprintf(sbuf,"con,");
     for(int i = 0; i <YOKUTAN_DATAS_NUM ; i++) {
-////        pc.printf("%i   ",soudaDatas[i]);
-        twe.printf("%i,",soudaDatas[i]);
-//        p = sprintf(sbuf+p,"%i,",soudaDatas[i]);
-//
+
+//        pc.printf("%i   ",soudaDatas[i]);
+//        twe.printf("%i,",soudaDatas[i]);
+        p += sprintf(sbuf+p,"%i,",soudaDatas[i]);
+
         if(i == YOKUTAN_DATAS_NUM - 1)
-            twe.printf("%i\n",soudaDatas[i]);
-//            p = sprintf(sbuf+p,"%i\n",soudaDatas[i]);
+//            twe.printf("%i\n",soudaDatas[i]);
+            p += sprintf(sbuf+p,"%i\n",soudaDatas[i]);
     }
 //    twe.printf("%s",sbuf);
     //twe.printf("con,%s\n",soudaDatas);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Thu Jun 15 08:33:31 2017 +0000
@@ -0,0 +1,446 @@
+//計器プログラム
+#include "mbed.h"
+#include "rtos.h"
+#include "Cadence.h"
+#include "Fusokukei.h"
+#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
+#define WRITE_DATAS_NUM 34 // souda_datas_num + 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
+#define ROLL_R_MAX_DEG 1.5
+#define ROLL_L_MAX_DEG 1.5
+#define MPU_DELT_MIN 250
+#define INIT_SERVO_PERIOD_MS 20
+
+//-----------------------------------(resetInterrupt def)
+extern "C" void mbed_reset();
+InterruptIn resetPin(p25);
+Timer resetTimeCount;
+void resetInterrupt()
+{
+    while(resetPin) {
+        resetTimeCount.start();
+        if(resetTimeCount.read()>3) mbed_reset();
+    }
+    resetTimeCount.reset();
+}
+//-------------------------------------------------------
+
+SDFileSystem sd(p5, p6, p7, p8, "sd");
+FILE* fp;
+
+//RawSerial pc(USBTX,USBRX);
+//Serial android(p9,p10);
+BufferedSoftSerial soudaSerial(p17,p18);
+BufferedSoftSerial twe(p11,p12);
+//Cadence cadence_twe(p13,p14);
+RawSerial android(p13,p14);
+
+Ticker cadenceUpdateTicker;
+//Ticker writeDatasTicker;
+//Timer writeTimer;
+
+InterruptIn FusokukeiPin(p24);
+Ticker FusokukeiTicker;
+Fusokukei air;
+volatile int air_kaitensu= 0;
+
+//Timer sonarTimer;
+AnalogIn sonarPin(p15);
+//InterruputIn sonarPin(p15);
+//double sonarDistTime
+double sonarDist;
+float sonarV;
+
+
+float sum = 0;
+uint32_t sumCount = 0;
+MPU6050 mpu6050;
+Timer t;
+Timer cadenceTimer;
+
+//Ticker mpu6050Ticker;
+
+DigitalOut RollAlarmR(p23);
+DigitalOut RollAlarmL(p22);
+DigitalOut led2(LED2);
+//DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+I2C InaI2c(p9,p10);
+INA226 VCmonitor(InaI2c,0x9C);
+AnalogIn mgPin(p20);
+//AnalogIn mgPin2(p16);
+InterruptIn cadenceInter(p16);
+
+char soudaDatas[SOUDA_DATAS_NUM];
+float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
+volatile int write_datas_index = 0;
+
+void air_countUp();
+void call_calcAirSpeed();
+void sonarInterruptStart();
+void sonarInterruptStop();
+void updateCadence(double source, double input,double input2);
+void init();
+void FusokukeiInit();
+void MpuInit();
+void mpuProcessing(void const *arg);
+void DataReceiveFromSouda(void const *arg);
+void SdInit();
+void SDprintf();
+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()
+{
+    air_kaitensu++;
+    led3 = !led3;
+}
+
+void call_calcAirSpeed()
+{
+    air.calcAirSpeed(air_kaitensu);
+    air_kaitensu = 0;
+}
+
+void sonarInterruptStart()
+{
+//    sonarTimer.start();
+}
+
+void sonarInterruptStop()
+{
+//    sonarTimer.stop();
+//    sonarDistTime = sonarTimer.read_us();
+//    sonarTimer.reset();
+//    sonarDist = sonarDistTime*0.018624 - 13.511;
+}
+void sonarCalc()
+{
+    sonarV = 0;
+    for(int i = 0; i<20; i++) {
+        sonarV += sonarPin.read();
+        wait(0.01);
+    }
+    sonarDist = (sonarV/20)*2064.5;// volt*3.3*1000/1.6 (電圧/距離:3.2mV/2cm)
+}
+
+
+// 定格12V電源の電圧値から定めた閾値を、oh182/E非接触回転速度センサ値が超えているかどうか
+// source: 定格12V電源の電圧値[mV], input: センサ値[mV]
+// return => 1:超えている, 0:超えていない, -1:エラー
+int isOh182eOverThreshold(double source, double input)
+{
+    double a, b;
+    if(source < 3200)
+        return -1;
+
+    if(source < 5500)
+        a = 0.233333333, b = -308.3333333;
+    else if(source < 7000)
+        a = 0.173333333, b = 21.66666667;
+    else
+        a = 0, b = 1235;
+
+    return (a * source + b < input) ? 1 : 0;
+}
+
+//ケイデンスの値を取得します。
+// source: 定格12V電源の電圧値[mV], input: センサ値[mV]
+void updateCadence()
+{
+    static bool isFFlag = true;
+    if(isFFlag) {
+        cadenceTimer.start();
+        isFFlag =  false;
+        return;
+    }
+    led4 = !led4;
+    if(cadenceCounter < 3) {
+        cadenceCounter++;
+        return;
+    }
+    cadenceResult =60.0/  (cadenceTimer.read_us() / 1000000.0); //クランク一回転にかかる時間を取得
+    cadenceTimer.reset();
+    cadenceCounter = 0;
+}
+
+void init()
+{
+    pc.printf("(BUILD:[" __DATE__ "/" __TIME__ "])\n\r");
+//--------------------------------------(resetInterrupt init)
+    resetPin.rise(resetInterrupt);
+    resetPin.mode(PullDown);
+//-----------------------------------------------------------
+    twe.baud(14400);//BufferedSoftSerialでは19200が上限。twelite側でもBPS無効化が必要
+    android.baud(9600);
+    //writeTimer.start();
+    FusokukeiInit();
+//    SdInit();
+//    MpuInit();
+    //writeDatasTicker.attach(&WriteDatas,1);
+
+//-----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");
+    }
+    VCmonitor.setCurrentCalibration();
+}
+
+void FusokukeiInit()
+{
+    FusokukeiPin.rise(air_countUp);
+    FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME);
+}
+
+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
+    if (whoami == 0x68) { // WHO_AM_I should always be 0x68
+        Thread::wait(100);
+        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
+        Thread::wait(100);
+        if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
+            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
+            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+            mpu6050.initMPU6050(); ////////////pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+            Thread::wait(200);
+        } else {
+        }
+    } else {
+        //////pc.printf("out\n\r"); // Loop forever if communication doesn't happen
+    }
+}
+
+double calcPulse(int deg)
+{
+    return (0.0006+(deg/180.0)*(0.00235-0.00045));
+}
+
+void mpuProcessing(void const *arg)
+{
+    MpuInit();
+    while(1) {
+        if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
+            mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
+            mpu6050.getAres();
+            ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
+            ay = (float)accelCount[1]*aRes - accelBias[1];
+            az = (float)accelCount[2]*aRes - accelBias[2];
+            mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
+            mpu6050.getGres();
+            gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
+            gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
+            gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
+            tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
+            temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
+        }
+        Now = t.read_us();
+        deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+        lastUpdate = Now;
+        sum += deltat;
+        sumCount++;
+        if(lastUpdate - firstUpdate > 10000000.0f) {
+            beta = 0.04;  // decrease filter gain after stabilized
+            zeta = 0.015; // increasey bias drift gain after stabilized
+        }
+        mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
+        delt_t = t.read_ms() - count;
+        if (delt_t > MPU_DELT_MIN) {
+            yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+            pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+            roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+            pitch *= 180.0f / PI;
+            yaw   *= 180.0f / PI;
+            roll  *= 180.0f / PI;
+            myled= !myled;
+            count = t.read_ms();
+            sum = 0;
+            sumCount = 0;
+        }
+        Thread::wait(1);
+    }//while(1)
+}
+
+void DataReceiveFromSouda(/*void const *arg*/)
+{
+//    while(1){
+    if(soudaSerial.readable()) {
+        led2 = !led2;
+        char c = soudaSerial.getc();
+        while( c != ';' ) {
+            c = soudaSerial.getc();
+        }
+        for(int i = 0; i < SOUDA_DATAS_NUM; i++) {
+            soudaDatas[i] = soudaSerial.getc();
+        }
+    }//if
+//    }//while(1)
+}
+
+void SdInit()
+{
+    mkdir("/sd/mydir", 0777);
+    fp = fopen("/sd/mydir/TestFlight.csv", "w");
+    if(fp == NULL) {
+        error("Could not open file for write\n");
+    }
+    fclose(fp);
+}
+
+void SDprintf(const void* arg)
+{
+    SdInit();
+    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++) {
+                    fprintf(fp,"%f,", writeDatas[i][j]);
+                }
+            }
+            fprintf(fp,"\n");
+            fclose(fp);
+
+            write_datas_index=0;
+        }
+        Thread::wait(1);
+    }
+}
+
+void WriteDatas()
+{
+    int i;
+    for(i = 0; i < SOUDA_DATAS_NUM; i++) {
+        //writeDatas[write_datas_index][i] = 0.0;
+        writeDatas[write_datas_index][i] = (float)soudaDatas[i];
+    }
+    writeDatas[write_datas_index][i++] = pitch;
+    writeDatas[write_datas_index][i++] = roll;
+    writeDatas[write_datas_index][i++] = yaw;
+    writeDatas[write_datas_index][i++] = airSpeed;
+    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]);
+//        twe.printf("%f,", writeDatas[write_datas_index][i]);
+//    }
+//    //pc.printf("\n\r");
+//    twe.printf("\n\r");
+    if(write_datas_index == SD_WRITE_NUM-1) {
+//        SDprintf();
+        write_datas_index=0;
+    } else {
+        write_datas_index++;
+    }
+//    char sbuf[128];
+//    int p=0;
+    twe.printf("con,");
+//    p = sprintf(sbuf,"con,");
+    for(int i = 0; i <YOKUTAN_DATAS_NUM ; i++) {
+////        pc.printf("%i   ",soudaDatas[i]);
+        twe.printf("%i,",soudaDatas[i]);
+//        p = sprintf(sbuf+p,"%i,",soudaDatas[i]);
+//
+        if(i == YOKUTAN_DATAS_NUM - 1)
+            twe.printf("%i\n",soudaDatas[i]);
+//            p = sprintf(sbuf+p,"%i\n",soudaDatas[i]);
+    }
+//    twe.printf("%s",sbuf);
+    //twe.printf("con,%s\n",soudaDatas);
+    /*
+    送信文字列
+    0-13翼端データ
+    14-17 R erebon
+    18 R DRUG
+    19-22 L erebon
+    23 LDRUG
+    */
+    twe.printf("inp,%d,%i,%d,%i\nmpu,%f,%f,%f\nkei,%f,%f,%f\n",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);//cadence_twe.cadence);
+    pc.printf("cadence:%5.2f\n\r",cadenceResult);
+    // 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);
+    if(android.writeable()) {
+        android.printf("%4.2f,%4.2f,%4.2f,\n,",roll,airSpeed,cadenceResult);//cadence_twe.cadence);
+    }
+//    SDprintf();
+}
+
+void WriteDatasF()
+{
+    //pc.printf("airSpeed:%f\n\r",airSpeed);
+}
+
+void RollAlarm()
+{
+    if((roll < -ROLL_L_MAX_DEG ) && (roll > ROLL_L_MAX_DEG-180)) {
+        RollAlarmL = 1;
+    } else {
+        RollAlarmL = 0;
+    }
+
+    if((roll > ROLL_R_MAX_DEG) && (roll < 180-ROLL_R_MAX_DEG)) {
+        RollAlarmR = 1;
+    } else {
+        RollAlarmR = 0;
+    }
+}
+
+//void updateCadenceFunc(void const *arg)
+//{
+//    while(1) {
+////        VCmonitor.getVoltage(&V) == 0;
+//        updateCadence(V,mgPin.read()*3300.0, mgPin2.read()*3300.0);
+//        Thread::wait(330);
+//    }
+//}
+int main()
+{
+    Thread mpu_thread(&mpuProcessing);
+    // Thread SD_thread(&SDprintf);
+    //Thread cadenceThread(&updateCadenceFunc);
+    cadenceInter.rise(&updateCadence);
+    cadenceInter.fall(&updateCadence);
+//    Thread soudaSerial_thread(&DataReceiveFromSouda);
+    init();
+    int counter = 0;
+    while(1) {
+        if(counter%50== 0) {
+            VCmonitor.getVoltage(&V);
+            printf("e:%f\n",V);
+        }
+        counter++;
+//        updateCadence(V,mgPin.read() * 3.3 * 1000.0,mgPin2.read() * 3.3* 1000.0);
+        sonarCalc();
+        RollAlarm();
+        DataReceiveFromSouda();
+        WriteDatas();
+        //   wait_ms(20);
+    }
+}
\ No newline at end of file