Diff: main.cpp

Revision:

8:31e07f6ed0f7

Parent:

7:9415448ae9ca

Child:

9:95eb0bbdc2a9

diff -r 9415448ae9ca -r 31e07f6ed0f7 main.cpp
--- a/main.cpp	Fri May 20 12:26:32 2016 +0000
+++ b/main.cpp	Fri May 27 06:42:37 2016 +0000
@@ -81,8 +81,8 @@
     kisokuServo.period_ms(INIT_SERVO_PERIOD_MS);
     geikakuServo.period_ms(INIT_SERVO_PERIOD_MS);
     FusokukeiInit();
-    //MpuInit(); 
-    //SdInit();
+    MpuInit(); 
+    SdInit();
     //writeDatasTicker.attach(&WriteDatas,1);
     //soudaSerial.attach(&DataReceiveFromSouda, Serial::RxIrq);
 }
@@ -183,61 +183,51 @@
     for(int i = 0; i < SD_WRITE_NUM; i++){
         for(int j = 0; j < WRITE_DATAS_NUM; j++){
             fprintf(fp,"%f,", writeDatas[i][j]);
-            //if(i%7==0){
-//                fprintf(fp,"\n\r");
-//            }
         }
     }
-    //for(int i = 0; i < SOUDA_DATAS_NUM; i++){
-//        fprintf(fp,"%i   ",soudaDatas[i]);
-//    }
-//    fprintf(fp, "p:%f,r:%f,y:%f\n",pitch,roll,yaw);
-//    fprintf(fp, "gx:%f,gy:%f,gz:%f\n",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
-//    fprintf(fp, "as:%f\n",airSpeed);
     fprintf(fp,"\n\r");
     fclose(fp);
 }
 
 void WriteDatas(){
-    pc.printf("%f\n\r",airSpeed);
-    //int i;
-//    for(i = 0; i < SOUDA_DATAS_NUM; i++){
-//        writeDatas[write_datas_index][i] = (float)soudaDatas[i];
-//    }
-//    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());
-//    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]);
-//        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++;
-//    } 
-        //for(int i = 0; i < SOUDA_DATAS_NUM; i++){
-//            pc.printf("%i   ",soudaDatas[i]);
-//            twe.printf("%i  ",soudaDatas[i]);
-//        }
-//        pc.printf("\n\r");
-//        twe.printf("\n\r");
-//        twe.printf("%f,%f,%f\n\r",pitch,roll,yaw);
-//        twe.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
-//        twe.printf("%f\n\r",airSpeed); 
-//        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);
-//        SDprintf();
+    int i;
+    for(i = 0; i < SOUDA_DATAS_NUM; i++){
+        writeDatas[write_datas_index][i] = (float)soudaDatas[i];
+    }
+    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());
+    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]);
+        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++;
+    } 
+        for(int i = 0; i < SOUDA_DATAS_NUM; i++){
+            pc.printf("%i   ",soudaDatas[i]);
+            twe.printf("%i  ",soudaDatas[i]);
+        }
+        pc.printf("\n\r");
+        twe.printf("\n\r");
+        twe.printf("%f,%f,%f\n\r",pitch,roll,yaw);
+        twe.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
+        twe.printf("%f\n\r",airSpeed); 
+        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);
+        SDprintf();
 }
 
 float calcKXdeg(float x){
@@ -276,10 +266,10 @@
 int main(){
     init();
     while(1){
-        //uProcessing();
-       // RollAlarm();
-        //taReceiveFromSouda();
+        mpuProcessing();
+        RollAlarm();
+        DataReceiveFromSouda();
         WriteDatas();
-        //iteServo();
+        WriteServo();
     }
 }
\ No newline at end of file