Diff: main.cpp

Revision:

4:67f705d42f1e

Parent:

3:c18342e4fddd

Child:

5:8bfe95431ec0

--- a/main.cpp	Wed Feb 06 11:49:05 2019 +0000
+++ b/main.cpp	Fri Feb 08 07:28:48 2019 +0000
@@ -4,8 +4,8 @@
 //MPU_check用
 #define PI 3.14159265358979
 
-#define servo_NEUTRAL_R    1460
-#define servo_NEUTRAL_L    1460
+#define servo_NEUTRAL_R    1616
+#define servo_NEUTRAL_L    1616
 #define servo_FORWARD_R    1860
 #define servo_FORWARD_L    1860
 #define servo_back_R       1060
@@ -36,10 +36,13 @@
 void setup();
 void Init_sensors();
 void DisplayClock();
+void DebugPrint();
 
 static float nowAngle[3] = {0,0,0};
 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0;
 
+bool setupFlag=false;
+
 enum Angle{ROLL, PITCH, YAW};   //yaw:北を0とした絶対角度
 
 Timer t;
@@ -80,9 +83,9 @@
         MoveCansat(g_landingcommand);
         
         SensingMPU();
+        DebugPrint();
+        
         wait_ms(23);
-        //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用
-        //pc.printf("\r\n");
         }
 }
 
@@ -270,6 +273,7 @@
     wait(0.2);
     
     pc.printf("All initialized\r\n");
+    setupFlag=true;
 }
 
 
@@ -299,7 +303,16 @@
     for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
     rpy[ROLL] -= FirstROLL;
     rpy[PITCH] -= FirstPITCH;
-    rpy[YAW] -= FirstYAW;
+    if(!setupFlag){
+        rpy[YAW] -= FirstYAW;
+        }else{
+        if(rpy[YAW] >= FirstYAW){
+            rpy[YAW] -= FirstYAW;
+            }else{
+            rpy[YAW] += 360.0f;
+            rpy[YAW] -= FirstYAW;
+        }
+    }
     
     for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}}
     if(!flg_checkoutlier || count_changeRPY >= 2){
@@ -326,4 +339,11 @@
     pc.printf("PCLK1 Clock  = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
     pc.printf("PCLK2 Clock  = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
     pc.printf("\r\n");
-}
\ No newline at end of file
+}
+
+void DebugPrint(){
+    //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用
+    pc.printf("%3.2f\t",nowAngle[2]);
+    pc.printf("\r\n");
+    }
+    
\ No newline at end of file