Revision 34:c46f2f687c7b, committed 2017-02-18

Comitter:

tsumagari

Date:

Sat Feb 18 06:45:59 2017 +0000

Branch:

Thread-gyogetsuMPU

Parent:

33:69ad9920f693

Child:

37:34aaa1951390

Commit message:

????????????

Changed in this revision

--- a/Cadence.h	Sat Feb 18 03:03:25 2017 +0000
+++ b/Cadence.h	Sat Feb 18 06:45:59 2017 +0000
@@ -2,9 +2,10 @@
 #define CADENCE_H
 
 #include "mbed.h"
-#include "BufferedSoftSerial.h"
+//#include "BufferedSoftSerial.h"
 #include <string>
 
+DigitalOut led3(LED3);
 double cadence;
 
 class Cadence : public /*BufferedSoft*/RawSerial{
@@ -30,26 +31,33 @@
         strC[0] = '0';
         strV[0] = '0';
     }
+    int checkInt(char c[]){
+        for(int i = 0; i< sizeof(c)/sizeof(c[0]); i++){
+            if( c[i] - '0' >9 || c[i] - '0' < 0 ) return -1;
+        }
+        return 1;
+    }
     void readData(){ //Ticker で定期的に呼び出して値を更新
-      if(readable()){ 
+//      if(readable()){ 
         data_count = 0;
         do{
             if(readable()){
                 data[data_count] = getc();
                 if(data[data_count] != '\n') data_count++;
+                led3 = !led3;
             }
         }while( data[data_count-1] != '\r' && data_count < DATAS_NUM );
         if( data_count > 71 ) {
             for(int i = 0; i<4; i++){
-                strC[i] = data[data_count - 6 + i]; // 7 = 5 + 1
-                strV[i] = data[data_count - 43 + i]; // 44 = 42 + 1
+                strC[i] = data[data_count - 6 + i]; // 6 = 5 + 1
+                strV[i] = data[data_count - 43 + i]; // 43 = 42 + 1
             }
-            sscanf(strC,"%4d",&cadence_i);
-            sscanf(strV,"%4d",&voltage_i);
+            if(checkInt(strC)) sscanf(strC,"%4d",&cadence_i);
+            if(checkInt(strV)) sscanf(strV,"%4d",&voltage_i);
             cadence = cadence_i/6.0;
             voltage = voltage_i/1000.0;
         }
-      }
+//      }
     }
 };
 #endif
\ No newline at end of file

--- a/MPU6050.h	Sat Feb 18 03:03:25 2017 +0000
+++ b/MPU6050.h	Sat Feb 18 06:45:59 2017 +0000
@@ -331,7 +331,7 @@
   writeByte(MPU6050_ADDRESS, MOT_THR, 0x80); // Set motion detection to 0.256 g; LSB = 2 mg
   writeByte(MPU6050_ADDRESS, MOT_DUR, 0x01); // Set motion detect duration to 1  ms; LSB is 1 ms @ 1 kHz rate
   
-  wait(0.1);  // Add delay for accumulation of samples
+  Thread::wait(100);  // Add delay for accumulation of samples
   
   c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG);
   writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x07); // Clear high-pass filter bits [2:0]
@@ -351,7 +351,7 @@
 void resetMPU6050() {
   // reset device
   writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
-  wait(0.1);
+  Thread::wait(100);
   }
   
   
@@ -360,7 +360,7 @@
  // Initialize MPU6050 device
  // wake up device
   writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors 
-  wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt  
+  Thread::wait(100); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt  
 
  // get stable time source
   writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01);  // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
@@ -404,13 +404,13 @@
   
 // reset device, reset all registers, clear gyro and accelerometer bias registers
   writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
-  wait(0.1);  
+  Thread::wait(100);  
    
 // get stable time source
 // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
   writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01);  
   writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00); 
-  wait(0.2);
+  Thread::wait(200);
   
 // Configure device for bias calculation
   writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00);   // Disable all interrupts
@@ -419,7 +419,7 @@
   writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
   writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00);    // Disable FIFO and I2C master modes
   writeByte(MPU6050_ADDRESS, USER_CTRL, 0x0C);    // Reset FIFO and DMP
-  wait(0.015);
+  Thread::wait(15);
   
 // Configure MPU6050 gyro and accelerometer for bias calculation
   writeByte(MPU6050_ADDRESS, CONFIG, 0x01);      // Set low-pass filter to 188 Hz
@@ -433,7 +433,7 @@
 // Configure FIFO to capture accelerometer and gyro data for bias calculation
   writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40);   // Enable FIFO  
   writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78);     // Enable gyro and accelerometer sensors for FIFO  (max size 1024 bytes in MPU-6050)
-  wait(0.08); // accumulate 80 samples in 80 milliseconds = 960 bytes
+  Thread::wait(80); // accumulate 80 samples in 80 milliseconds = 960 bytes
 
 // At end of sample accumulation, turn off FIFO sensor read
   writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00);        // Disable gyro and accelerometer sensors for FIFO
@@ -550,7 +550,7 @@
    // Configure the accelerometer for self-test
    writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0xF0); // Enable self test on all three axes and set accelerometer range to +/- 8 g
    writeByte(MPU6050_ADDRESS, GYRO_CONFIG,  0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
-   wait(0.25);  // Delay a while to let the device execute the self-test
+   Thread::wait(250);  // Delay a while to let the device execute the self-test
    rawData[0] = readByte(MPU6050_ADDRESS, SELF_TEST_X); // X-axis self-test results
    rawData[1] = readByte(MPU6050_ADDRESS, SELF_TEST_Y); // Y-axis self-test results
    rawData[2] = readByte(MPU6050_ADDRESS, SELF_TEST_Z); // Z-axis self-test results

--- a/main.cpp	Sat Feb 18 03:03:25 2017 +0000
+++ b/main.cpp	Sat Feb 18 06:45:59 2017 +0000
@@ -6,7 +6,7 @@
 #include "BufferedSoftSerial.h"
 #include "Cadence.h"
 
-#define SOUDA_DATAS_NUM 18
+#define SOUDA_DATAS_NUM 24 //(yokutan 7 + input 5)*2
 #define WRITE_DATAS_NUM 20
 #define SD_WRITE_NUM 1
 #define MPU_LOOP_TIME 0.01
@@ -35,16 +35,16 @@
 BufferedSoftSerial soudaSerial(p17,p18);
 BufferedSoftSerial twe(p11,p12);
 Cadence cadence_twe(p13,p14);
-Ticker cadenceUpdateTicker;
-Ticker writeDatasTicker;
-Timer writeTimer;
+//Ticker cadenceUpdateTicker;
+//Ticker writeDatasTicker;
+//Timer writeTimer;
 
 InterruptIn FusokukeiPin(p23);
 Ticker FusokukeiTicker;
 Fusokukei air;
 volatile int air_kaitensu= 0;
 
-Timer sonarTimer;
+//Timer sonarTimer;
 AnalogIn sonarPin(p15);
 //InterruputIn sonarPin(p15);
 //double sonarDistTime
@@ -56,7 +56,7 @@
 uint32_t sumCount = 0;
 MPU6050 mpu6050;
 Timer t;
-Ticker mpu6050Ticker;
+//Ticker mpu6050Ticker;
 
 DigitalOut RollAlarmR(p20);
 DigitalOut RollAlarmL(p19);
@@ -71,11 +71,12 @@
 void call_calcAirSpeed();
 void sonarInterruptStart();
 void sonarInterruptStop();
+void updateCadence(void const *arg);
 void init();
 void FusokukeiInit();
 void MpuInit();
-void mpuProcessing();
-void DataReceiveFromSouda();
+void mpuProcessing(void const *arg);
+void DataReceiveFromSouda(void const *arg);
 void WriteDatas();
 float calcAttackAngle();
 float calcKXdeg(float x);
@@ -90,7 +91,7 @@
 }
 
 void sonarInterruptStart(){
-    sonarTimer.start();
+//    sonarTimer.start();
 }
 
 void sonarInterruptStop(){
@@ -108,11 +109,11 @@
     sonarDist = (sonarV/20)*2064.5;// volt*3.3*1000/1.6 (電圧/距離:3.2mV/2cm)
 }
 
-void updateCadence(){
-    while(1){
+void updateCadence(/*void const *arg*/){
+//    while(1){
         cadence_twe.readData();
-        Thread::wait(0.1);
-    }
+//        Thread::wait(0.5);
+//    }
 }
 
 void init(){
@@ -143,14 +144,14 @@
     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);
+        Thread::wait(100);
         mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
-        Thread::wait(1);
+        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(2);
+            Thread::wait(200);
         } else {
         }
     } else {
@@ -163,7 +164,7 @@
 
 }
 
-void mpuProcessing(){
+void mpuProcessing(void const *arg){
     MpuInit();
     while(1){
         if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
@@ -207,15 +208,19 @@
     }//while(1)
 }
 
-void DataReceiveFromSouda(){
-    led2 = !led2;
-    char c = soudaSerial.getc();
-    while( c != ';' ){
-        c = soudaSerial.getc();
-    }
-    for(int i = 0; i < SOUDA_DATAS_NUM; i++){
-        soudaDatas[i] = soudaSerial.getc();
-    }
+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 WriteDatas(){
@@ -279,14 +284,14 @@
 //}
 
 void RollAlarm(){ 
-    if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){
+    if((roll < -ROLL_L_MAX_DEG ) && (roll > ROLL_L_MAX_DEG-180)){
         RollAlarmL = 1;
     }
     else{
         RollAlarmL = 0;
     }
     
-    if((roll > 0) && (roll < 180-ROLL_R_MAX_DEG)){
+    if((roll > ROLL_R_MAX_DEG) && (roll < 180-ROLL_R_MAX_DEG)){
         RollAlarmR = 1;
     }
     else{
@@ -312,6 +317,7 @@
 int main(){
 //    Thread cadence_thread(&updateCadence);
     Thread mpu_thread(&mpuProcessing);
+//    Thread soudaSerial_thread(&DataReceiveFromSouda);
     init();
     while(1){
         pc.printf("test\n\r");
@@ -319,7 +325,7 @@
         sonarCalc();
         RollAlarm();
         DataReceiveFromSouda();
-//        updateCadence();
+        updateCadence();
         WriteDatas();
 //        WriteServo();
     }