Diff: main.cpp

Revision:

26:50272431cd1e

Parent:

23:e59afb2044df

Child:

27:d2955f29a3aa

Child:

29:2da9b8d03c0b

Child:

43:0d7d2e703523

--- a/main.cpp	Sat Jan 07 06:45:20 2017 +0000
+++ b/main.cpp	Wed Jan 25 12:24:10 2017 +0000
@@ -10,44 +10,72 @@
 
 #define KX_VALUE_MIN 0.4
 #define KX_VALUE_MAX 0.8
-#define SOUDA_DATAS_NUM 12
-#define WRITE_DATAS_NUM 20
-#define MPU_LOOP_TIME 0.01
+#define SOUDA_DATAS_NUM 18
+#define YOKUTAN_DATAS_NUM 14
+#define WRITE_DATAS_NUM 18
 #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 10
+#define SD_WRITE_NUM 20
 #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);
+Timer resetTimeCount;
+void resetInterrupt(){
+    while(resetPin){
+        resetTimeCount.start();
+        if(resetTimeCount.read()>3) mbed_reset();
+    }
+    resetTimeCount.reset();
+}
+//-------------------------------------------------------
+
+
 Cadence cadence(p13,p14);
-//Ticker cadenceTicker;
 
 RawSerial pc(USBTX,USBRX);
-RawSerial Android(p13,p14);
-BufferedSoftSerial twe(p9,p10);
+//RawSerial Android(p13,p14);
+BufferedSoftSerial twe(p11,p12);
+RawSerial Android(p9,p10);
 BufferedSoftSerial soudaSerial(p17,p18);
 //Ticker writeDatasTicker;
 Timer writeTimer;
 
-InterruptIn FusokukeiPin(p21);
+InterruptIn FusokukeiPin(p22);
 Ticker FusokukeiTicker;
 Fusokukei air;
 volatile int air_kaitensu= 0;
 
+float sum = 0;
+uint32_t sumCount = 0;
 MPU6050 mpu6050;
 Timer t;
-Ticker mpu6050Ticker;
+//
+//AnalogIn kx_X(p17);
+//AnalogIn kx_Y(p16);
+//AnalogIn kx_Z(p15);
+//float KX_X,KX_Y,KX_Z;
 
-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;
 
 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");
@@ -66,14 +94,20 @@
 void init();
 void FusokukeiInit();
 void SdInit();
+void MpuInit();
+//void SonarInit();
+void mpuProcessing();
+void DataReceiveFromSouda();
 void SDprintf(void const *argument);
-void DataReceiveFromSouda();
 void WriteDatas();
-float calcAttackAngle();
-float calcKXdeg(float x);
+//float calcAttackAngle();
+//float calcKXdeg(float x);
 
 void cadenceDataReceive(){
-//    cadence.readData();
+    while(1){
+        cadence.readData();
+        Thread::wait(0.01);
+    }
 }
 
 void air_countUp(){
@@ -85,15 +119,31 @@
     air_kaitensu = 0;
 }
 
+//void sonarInterruptStart(){
+//    sonarTimer.start();
+//    led3 = 1;
+//}
+//void sonarInterruptStop(){
+//    sonarTimer.stop();
+//    led3 = 0;
+//    sonarDistTime = sonarTimer.read_us();
+//    sonarTimer.reset();
+//}
+
 void init(){
-    twe.baud(19200);
+//--------------------------------------(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);
     FusokukeiInit();
-    mpu6050.MPUInit(t); 
-//    SdInit();//thread 内で
+//    MpuInit(); //mpuProcessing内で
+//    SonarInit();
+//    SdInit();//SDprintf 内で
 }
 
 void FusokukeiInit(){
@@ -115,14 +165,99 @@
     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);
+        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
+        Thread::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);
+        } else {
+        }
+    } else {
+        pc.printf("MPU6050 not ready...\n\r");
+        //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
+    }   
+}
 
-void DataReceiveFromSouda(){
-    led2 = !led2;
-    for(int i = 0; i < SOUDA_DATAS_NUM; i++){
-        if(soudaSerial.readable()) {
-            soudaDatas[i] = (char)soudaSerial.getc();
-            if(soudaDatas[i]==';') i=-1;
-        }else i--;
+//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);
+    }
+}
+
+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(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--;
+        }
     }
 }
 
@@ -130,7 +265,7 @@
     SdInit();
     while(1){
         if(write_datas_index == SD_WRITE_NUM-1){
-            led4 = !led4;
+//            led4 = !led4;
             fp = fopen("/sd/mydir/sdtest.csv", "a");
             if(fp == NULL) {
                 error("Could not open file for write\n");
@@ -144,37 +279,45 @@
             fclose(fp);
             write_datas_index=0;
         }
-        Thread::wait(10);
+        Thread::wait(0.01);
     }
 }
 
 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];
+    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;
     }
-//    writeDatas[write_datas_index][i++] = cadence.cadence;
-    writeDatas[write_datas_index][i++] = calcKXdeg(kx_X.read());
-    writeDatas[write_datas_index][i++] = calcKXdeg(kx_Y.read());
-    writeDatas[write_datas_index][i++] = calcKXdeg(kx_Z.read());
+    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++] = 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++;
     } 
     for(int i = 0; i < SOUDA_DATAS_NUM; i++){
-        pc.printf("%i   ",soudaDatas[i]);
+//        pc.printf("%i   ",soudaDatas[i]);
+        ssMutex.lock();
         twe.printf("%i,",soudaDatas[i]);
+        ssMutex.unlock();
     }
 //    twe.printf("%f\n\r",cadence.cadence);
 //    pc.printf("%f\n\r",cadence.cadence);
@@ -183,12 +326,14 @@
 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,%f,%f,",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
     twe.printf("%f,\r\n",airSpeed); 
-    pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
+    ssMutex.unlock();
+//    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\n\r",airSpeed);
     //SDprintf();
     pc.printf("%d\n\r",write_datas_index);
 }
@@ -196,14 +341,14 @@
 void WriteDatasF(){
     pc.printf("airSpeed:%f\n\r",airSpeed);
 }
+//
+//float calcKXdeg(float x){
+//    return -310.54*x+156.65;
+//}
 
-float calcKXdeg(float x){
-    return -310.54*x+156.65;
-}
-
-float calcAttackAngle(){
-    return pitch-calcKXdeg(kx_Z.read());
-}
+//float calcAttackAngle(){
+//    return pitch-calcKXdeg(kx_Z.read());
+//}
 
 void RollAlarm(){ 
     if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){
@@ -220,26 +365,21 @@
     }
 }
 
-void WriteServo(){
-    kisokuServo.pulsewidth(calcPulse(9*airSpeed));
-    if(pitch<0){
-        geikakuServo.pulsewidth(calcPulse(0));
-    }
-    else{
-        geikakuServo.pulsewidth(calcPulse(abs(pitch*90/13.0)));
-    }
-}
+
 
 int main(){
     Thread sd_thread(&SDprintf);//必ずmain内で
+    Thread sonar_thread(&sonarCalc);
+    Thread mpu_thread(&mpuProcessing);
+    Thread soudaSerial_thread(&DataReceiveFromSouda);
+    Thread cadence_thread(&cadenceDataReceive);
     init();
     while(1){
         pc.printf("test\n\r");
-        mpu6050.mpuProcessing(t);
+//        mpuProcessing();
         RollAlarm();
-        DataReceiveFromSouda();
+//        DataReceiveFromSouda();
 //        cadence.readData();
         WriteDatas();
-        WriteServo();
     }
 }
\ No newline at end of file