Revision 9:95eb0bbdc2a9, committed 2016-08-12

Comitter:

taurin

Date:

Fri Aug 12 08:49:08 2016 +0000

Parent:

8:31e07f6ed0f7

Commit message:

rtos???;

Changed in this revision

diff -r 31e07f6ed0f7 -r 95eb0bbdc2a9 main.cpp
--- a/main.cpp	Fri May 27 06:42:37 2016 +0000
+++ b/main.cpp	Fri Aug 12 08:49:08 2016 +0000
@@ -13,7 +13,7 @@
 #define WRITE_DATAS_LOOP_TIME 1
 #define ROLL_R_MAX_DEG 2
 #define ROLL_L_MAX_DEG 2
-#define MPU_DELT_MIN 500
+#define MPU_DELT_MIN 250
 #define SD_WRITE_NUM 10
 #define INIT_SERVO_PERIOD_MS 20
 
@@ -192,6 +192,7 @@
 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];
     }
     writeDatas[write_datas_index][i++] = calcKXdeg(kx_X.read());
@@ -202,12 +203,12 @@
     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");
+    //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;
@@ -215,19 +216,23 @@
     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();
+    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();
+}
+
+void WriteDatasF(){
+    pc.printf("airSpeed:%f\n\r",airSpeed);
 }
 
 float calcKXdeg(float x){
@@ -255,8 +260,14 @@
 }
 
 void WriteServo(){
-    kisokuServo.pulsewidth(calcPulse(airSpeed*10));
-    geikakuServo.pulsewidth(calcPulse(pitch*90/13.0));
+    //kisokuServo.pulsewidth(calcPulse(airSpeed*10));
+    kisokuServo.pulsewidth(calcPulse(9*airSpeed));
+    if(pitch<0){
+        geikakuServo.pulsewidth(calcPulse(0));
+    }
+    else{
+        geikakuServo.pulsewidth(calcPulse(abs(pitch*90/13.0)));
+    }
     //pc.printf("a:%f",airSpeed);
     //pc.printf("p:%f\r\n",pitch);
     //kisokuServo.pulsewidth(calcPulse(0));
@@ -266,6 +277,7 @@
 int main(){
     init();
     while(1){
+        pc.printf("test\n\r");
         mpuProcessing();
         RollAlarm();
         DataReceiveFromSouda();