Diff: main.cpp

Branch:

noThread2017ver.

Revision:

27:d2955f29a3aa

Parent:

26:50272431cd1e

Child:

28:02e21f1fbe3d

Child:

33:69ad9920f693

--- a/main.cpp	Wed Jan 25 12:24:10 2017 +0000
+++ b/main.cpp	Sat Jan 28 10:17:50 2017 +0000
@@ -1,30 +1,22 @@
 //計器プログラム
-
 #include "mbed.h"
-#include "rtos.h"
+//#include "rtos.h"
 #include "Fusokukei.h"
 #include "MPU6050.h"
-#include "SDFileSystem.h"
 #include "BufferedSoftSerial.h"
 #include "Cadence.h"
 
-#define KX_VALUE_MIN 0.4
-#define KX_VALUE_MAX 0.8
 #define SOUDA_DATAS_NUM 18
-#define YOKUTAN_DATAS_NUM 14
-#define WRITE_DATAS_NUM 18
+#define WRITE_DATAS_NUM 20
+#define SD_WRITE_NUM 1
+#define MPU_LOOP_TIME 0.01
 #define AIR_LOOP_TIME 0.01
 #define WRITE_DATAS_LOOP_TIME 1
 #define ROLL_R_MAX_DEG 2
 #define ROLL_L_MAX_DEG 2
-#define SD_WRITE_NUM 20
+#define MPU_DELT_MIN 250
 #define INIT_SERVO_PERIOD_MS 20
 
-#define MPU_LOOP_TIME 0.01
-#define MPU_DELT_MIN 250
-
-Mutex ssMutex;
-
 //-----------------------------------(resetInterrupt def)
 extern "C" void mbed_reset();
 InterruptIn resetPin(p26);
@@ -38,77 +30,55 @@
 }
 //-------------------------------------------------------
 
-
-Cadence cadence(p13,p14);
-
 RawSerial pc(USBTX,USBRX);
-//RawSerial Android(p13,p14);
+RawSerial android(p9,p10);
+BufferedSoftSerial soudaSerial(p17,p18);
 BufferedSoftSerial twe(p11,p12);
-RawSerial Android(p9,p10);
-BufferedSoftSerial soudaSerial(p17,p18);
-//Ticker writeDatasTicker;
+Cadence cadence_twe(p13,p14);
+Ticker cadenceUpdateTicker;
+Ticker writeDatasTicker;
 Timer writeTimer;
 
-InterruptIn FusokukeiPin(p22);
+InterruptIn FusokukeiPin(p23);
 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;
-//
-//AnalogIn kx_X(p17);
-//AnalogIn kx_Y(p16);
-//AnalogIn kx_Z(p15);
-//float KX_X,KX_Y,KX_Z;
-
-
-//Timer sonarTimer;
-//InterruptIn sonarPin(p22);
-AnalogIn sonarPin(p15);
-//double sonarDistTime=0;
-double sonarV = 0.0, sonarDist = 0.0;
+Ticker mpu6050Ticker;
 
 DigitalOut RollAlarmR(p20);
 DigitalOut RollAlarmL(p19);
-//DigitalOut led(LED1);
+DigitalOut led(LED1);
 DigitalOut led2(LED2);
-DigitalOut led3(LED3);
-DigitalOut led4(LED4);
-
-SDFileSystem sd(p5, p6, p7, p8, "sd");
-FILE* fp;
-
-PwmOut kisokuServo(p26);
-PwmOut geikakuServo(p22);
 
 char soudaDatas[SOUDA_DATAS_NUM];
 float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
 volatile int write_datas_index = 0;
 
-void cadenceDataReceive();
 void air_countUp();
 void call_calcAirSpeed();
+void sonarInterruptStart();
+void sonarInterruptStop();
 void init();
 void FusokukeiInit();
-void SdInit();
 void MpuInit();
-//void SonarInit();
 void mpuProcessing();
 void DataReceiveFromSouda();
-void SDprintf(void const *argument);
 void WriteDatas();
-//float calcAttackAngle();
-//float calcKXdeg(float x);
-
-void cadenceDataReceive(){
-    while(1){
-        cadence.readData();
-        Thread::wait(0.01);
-    }
-}
+float calcAttackAngle();
+float calcKXdeg(float x);
 
 void air_countUp(){
     air_kaitensu++;
@@ -119,31 +89,45 @@
     air_kaitensu = 0;
 }
 
-//void sonarInterruptStart(){
-//    sonarTimer.start();
-//    led3 = 1;
-//}
-//void sonarInterruptStop(){
+void sonarInterruptStart(){
+    sonarTimer.start();
+}
+
+void sonarInterruptStop(){
 //    sonarTimer.stop();
-//    led3 = 0;
 //    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)
+}
+
+void updateCadence(){
+        cadence_twe.readData();
+}
 
 void init(){
 //--------------------------------------(resetInterrupt init)
     resetPin.rise(resetInterrupt);
     resetPin.mode(PullDown);
 //-----------------------------------------------------------
-    twe.baud(115200);
-    Android.baud(9600);
-    soudaSerial.baud(9600);
-    kisokuServo.period_ms(INIT_SERVO_PERIOD_MS);
-    geikakuServo.period_ms(INIT_SERVO_PERIOD_MS);
+    twe.baud(19200);//BufferedSoftSerialでは19200が上限。twelite側でもBPS無効化が必要
+    //writeTimer.start();
     FusokukeiInit();
-//    MpuInit(); //mpuProcessing内で
-//    SonarInit();
-//    SdInit();//SDprintf 内で
+    MpuInit(); 
+    //writeDatasTicker.attach(&WriteDatas,1);
+//    cadenceUpdateTicker.attach(&updateCadence, 1);
+    
+//-----for InterruptMode of sonar----------------------------
+//    sonarPin.rise(&sonarInterruptStart);
+//    sonarPin.fall(&sonarInterruptStop);
+//-----------------------------------------------------------
 }
 
 void FusokukeiInit(){
@@ -151,197 +135,134 @@
     FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME);
 }
 
-double calcPulse(int deg){
-    return (0.0006+(deg/180.0)*(0.00235-0.00045));
-}
-
-void SdInit(){
-    mkdir("/sd/mydir", 0777);
-    fp = fopen("/sd/mydir/sdtest2.csv", "w");
-    if(fp == NULL) {
-        error("Could not open file for write\n");
-    }
-    fprintf(fp, "Hello fun SD Card World!\n\r");
-    fclose(fp);
-//    Thread sd_thread(&SDprintf);
-}
 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(1);
+        wait(1);
         mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
-        Thread::wait(1);
+        wait(1);
         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(2);
+            wait(2);
         } else {
         }
     } else {
-        pc.printf("MPU6050 not ready...\n\r");
         //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
     }   
 }
 
-//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);
-        }
-        sonarDist = (sonarV/20)*2062.5;
-        Thread::wait(0.01);
+double calcPulse(int deg){
+    return (0.0006+(deg/180.0)*(0.00235-0.00045));
+
+}
+
+void mpuProcessing(){
+    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;
     }
 }
 
-void mpuProcessing(void const *argument){
-    MpuInit();
-    while(1){
-        Thread::wait(0.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;
-        }
+void DataReceiveFromSouda(){
+    led2 = !led2;
+    char c = soudaSerial.getc();
+    while( c != ';' ){
+        c = soudaSerial.getc();
     }
-}
-
-void DataReceiveFromSouda(void const *argument){
-    while(1){
-        led2 = !led2;
-        for(int i = 0; i < SOUDA_DATAS_NUM; i++){
-            if(soudaSerial.readable()) {
-//                ssMutex.lock();
-                soudaDatas[i] = (char)soudaSerial.getc();
-//                ssMutex.unlock();
-                if(soudaDatas[i]==';') i=-1;
-            }else i--;
-        }
-    }
-}
-
-void SDprintf(void const *argument){
-    SdInit();
-    while(1){
-        if(write_datas_index == SD_WRITE_NUM-1){
-//            led4 = !led4;
-            fp = fopen("/sd/mydir/sdtest.csv", "a");
-            if(fp == NULL) {
-                error("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\r");
-            fclose(fp);
-            write_datas_index=0;
-        }
-        Thread::wait(0.01);
+    for(int i = 0; i < SOUDA_DATAS_NUM; i++){
+        soudaDatas[i] = soudaSerial.getc();
     }
 }
 
 void WriteDatas(){
     int i;
-    for(i = 0; i < 6; i++){ //翼端のmpu
-        if(!(i%2)){
-            writeDatas[write_datas_index][i] = (int)( (soudaDatas[i*2]<<8) + soudaDatas[(i+1)*2] );
-        }else{
-            writeDatas[write_datas_index][i] = (int)( (soudaDatas[i*2-1]<<8) + soudaDatas[i*2+1] );
-        }
-//        writeDatas[write_datas_index][i] = i;
+    for(i = 0; i < SOUDA_DATAS_NUM; i++){
+        //writeDatas[write_datas_index][i] = 0.0;    
+        writeDatas[write_datas_index][i] = (float)soudaDatas[i];
     }
-    for(i = 6; i < 12; i++){//翼端のV,操舵
-        //writeDatas[write_datas_index][i] = 0.0;    
-        writeDatas[write_datas_index][i] = (float)soudaDatas[i+6];
-    }
-    writeDatas[write_datas_index][i++] = cadence.cadence;
-    writeDatas[write_datas_index][i++] = sonarDist;
     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++] = 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++){
+//        pc.printf("%f   ", writeDatas[write_datas_index][i]);
 //        twe.printf("%f,", writeDatas[write_datas_index][i]);
-    }
-    pc.printf("\n\r");
+//    }
+//    pc.printf("\n\r");
 //    twe.printf("\n\r");
-    if(write_datas_index < SD_WRITE_NUM-1){
-        write_datas_index++;
-    } 
+//    if(write_datas_index == SD_WRITE_NUM-1){
+//        write_datas_index=0;
+//    }
+//    else{
+//        write_datas_index++;
+//    } 
     for(int i = 0; i < SOUDA_DATAS_NUM; i++){
-//        pc.printf("%i   ",soudaDatas[i]);
-        ssMutex.lock();
+        pc.printf("%i   ",soudaDatas[i]);
         twe.printf("%i,",soudaDatas[i]);
-        ssMutex.unlock();
+//        android.printf("%i,",soudaDatas[i]);
     }
-//    twe.printf("%f\n\r",cadence.cadence);
-//    pc.printf("%f\n\r",cadence.cadence);
     //pc.printf("\n\r");
-
-if(Android.writeable()){
-    Android.printf("%f,%f,%f",airSpeed,roll,0);
-}
-    ssMutex.lock();
     twe.printf("%f,%f,%f,",pitch,roll,yaw);
-//    twe.printf("%f,%f,%f,",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
-    twe.printf("%f,\r\n",airSpeed); 
-    ssMutex.unlock();
-//    pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
+    twe.printf("%f,%f,%f\r\n",airSpeed,sonarDist,cadence); 
+    pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
     //pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
-//    pc.printf("%f\n\r",airSpeed);
+    pc.printf("%f,%f,%f\n\r",airSpeed,sonarDist,cadence);
+    pc.putc(cadence_twe.strV[0]);
+    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("%f,%f,test\n\r",roll,airSpeed);
+    }
     //SDprintf();
-    pc.printf("%d\n\r",write_datas_index);
 }
 
 void WriteDatasF(){
     pc.printf("airSpeed:%f\n\r",airSpeed);
 }
-//
+
 //float calcKXdeg(float x){
 //    return -310.54*x+156.65;
 //}
@@ -357,6 +278,7 @@
     else{
         RollAlarmL = 0;
     }
+    
     if((roll > 0) && (roll < 180-ROLL_R_MAX_DEG)){
         RollAlarmR = 1;
     }
@@ -365,21 +287,32 @@
     }
 }
 
-
+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 sd_thread(&SDprintf);//必ずmain内で
-    Thread sonar_thread(&sonarCalc);
-    Thread mpu_thread(&mpuProcessing);
-    Thread soudaSerial_thread(&DataReceiveFromSouda);
-    Thread cadence_thread(&cadenceDataReceive);
+//    Thread cadence_thread(&updateCadence);
     init();
     while(1){
         pc.printf("test\n\r");
-//        mpuProcessing();
+        mpuProcessing();
+        sonarCalc();
         RollAlarm();
-//        DataReceiveFromSouda();
-//        cadence.readData();
+        DataReceiveFromSouda();
+        updateCadence();
         WriteDatas();
+//        WriteServo();
     }
 }
\ No newline at end of file