Diff: main.cpp

Revision:

5:1b3c8e5ee99e

Parent:

4:a863a092141c

Child:

6:98a63127aa11

--- a/main.cpp	Thu Mar 24 09:50:51 2016 +0000
+++ b/main.cpp	Fri Mar 25 01:22:17 2016 +0000
@@ -7,7 +7,7 @@
 #define KX_VALUE_MIN 0.4
 #define KX_VALUE_MAX 0.8
 #define SOUDA_DATAS_NUM 13
-#define WRITE_DATAS_NUM 21
+#define WRITE_DATAS_NUM 20
 #define MPU_LOOP_TIME 0.01
 #define AIR_LOOP_TIME 0.01
 #define WRITE_DATAS_LOOP_TIME 1
@@ -47,8 +47,8 @@
 SDFileSystem sd(p5, p6, p7, p8, "sd");
 FILE* fp;
 
-PwmOut drugServo(p22);
-PwmOut eruronServo(p23);
+PwmOut kisokuServo(p22);
+PwmOut geikakuServo(p25);
 
 char soudaDatas[SOUDA_DATAS_NUM];
 float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
@@ -78,8 +78,8 @@
 void init(){
     twe.baud(115200);
     writeTimer.start();
-    drugServo.period_ms(INIT_SERVO_PERIOD_MS);
-    eruronServo.period_ms(INIT_SERVO_PERIOD_MS);
+    kisokuServo.period_ms(INIT_SERVO_PERIOD_MS);
+    geikakuServo.period_ms(INIT_SERVO_PERIOD_MS);
     FusokukeiInit();
     MpuInit(); 
     SdInit();
@@ -108,7 +108,7 @@
         } else {
         }
     } else {
-        pc.printf("out\n\r"); // Loop forever if communication doesn't happen
+        //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
     }   
 }
 
@@ -150,7 +150,6 @@
         pitch *= 180.0f / PI;
         yaw   *= 180.0f / PI;
         roll  *= 180.0f / PI;
-        pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
         myled= !myled;
         count = t.read_ms();
         sum = 0;
@@ -170,7 +169,7 @@
 
 void DataReceiveFromSouda(){
     led2 = !led2;
-    pc.printf("received\n\r");
+    //pc.printf("received\n\r");
     for(int i = 0; i < SOUDA_DATAS_NUM; i++){
         soudaDatas[i] = soudaSerial.getc();
     }
@@ -183,10 +182,10 @@
     }
     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");
-            }
+            fprintf(fp,"%f,", writeDatas[i][j]);
+            //if(i%7==0){
+//                fprintf(fp,"\n\r");
+//            }
         }
     }
     //for(int i = 0; i < SOUDA_DATAS_NUM; i++){
@@ -195,12 +194,12 @@
 //    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(){
     int i;
-    pc.printf("testw\n\r");
     for(i = 0; i < SOUDA_DATAS_NUM; i++){
         writeDatas[write_datas_index][i] = (float)soudaDatas[i];
     }
@@ -211,15 +210,13 @@
     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();
+    //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]);
-        if(i%7==0){
-            pc.printf("\n\r");
-            twe.printf("\n\r");
-        }
+        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;
@@ -267,8 +264,11 @@
 }
 
 void WriteServo(){
-    eruronServo.pulsewidth(calcPulse(airSpeed*10));
-    drugServo.pulsewidth(calcPulse());
+    kisokuServo.pulsewidth(calcPulse(airSpeed*10));
+    //geikakuServo.pulsewidth(calcPulse(pitch*0.13));
+    
+    //kisokuServo.pulsewidth(calcPulse(0));
+    //geikakuServo.pulsewidth(calcPulse(0));
 }
 
 int main(){