Diff: main.cpp

Branch:

Thread-gyogetsuMPU

Revision:

34:c46f2f687c7b

Parent:

33:69ad9920f693

Child:

37:34aaa1951390

--- 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();
     }