Diff: main.cpp

Revision:

27:61876b34ded4

Parent:

26:bc185a3d16b6

Child:

28:aa44903a01e1

--- a/main.cpp	Fri Sep 21 04:26:30 2018 +0000
+++ b/main.cpp	Fri Sep 21 13:11:19 2018 +0000
@@ -50,6 +50,8 @@
 #define LEFT_ROLL_SHORT 12.0
 #define LEFT_PITCH_SHORT -5.0
 
+#define rightloopROLL2 -10.0
+
 /*#define rightloopRUD 1300  //1250
 #define rightloopshortRUD 1250 
 #define leftloopRUD  1500
@@ -185,6 +187,7 @@
 float g_SerialTargetYAW;
 
 Timer t;
+Timer t2;
 Timeout RerurnChickenServo1;
 Timeout RerurnChickenServo2;
 
@@ -1442,16 +1445,33 @@
 //右旋回
 void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回
 
-    targetAngle[ROLL] = g_rightloopROLL;
     targetAngle[PITCH] = g_rightloopPITCH ;
     autopwm[RUD]=g_rightloopRUD;              //RUD固定
     autopwm[THR] = SetTHRinRatio(0.5);                  //手動スロットル記憶
+    
+    
+    if (nowAngle[ROLL]>20.0||nowAngle[ROLL]<-20.0){
+        t2.start();
+        pc.printf("Timer start.");
+    }
+    if(0.0<t2.read()<5.0){
+            pc.printf("tagetAngle is changed.");
+            targetAngle[ROLL] = rightloopROLL2;
+            }
+        else {
+            t2.stop();
+            t2.reset();
+            pc.printf("Timer stopped.");
+            targetAngle[ROLL] = g_rightloopROLL;
+        }    
+
     if(autopwm[AIL_R]<trimpwm[AIL_R]){                                                                                  //エルロン上がりやすさ調節
         autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
         }
     else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
     autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
     
+    
     //checkHeight(targetAngle);
     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
 }
@@ -1553,12 +1573,12 @@
     //for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]);
     //for(uint8_t i=1; i<4; i++) pc.printf("%d ",sbus.manualpwm[i]);
     //pc.printf("\r\n");
-    //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
+    for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
     //for(uint8_t i=0; i<2; i++) pc.printf("%3.2f\t",nowAngle[i]); //ロール、ピッチのみ　小数点以下2ケタ
     //pc.printf("%d\t",autopwm[AIL_L]); // pc.printf("%d\t",autopwm[RUD]);
     //pc.printf("%d",g_distance);
 
     //pc.printf("Mode: %c: ",g_buf[0]);
     //if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW);
-    //pc.printf("\r\n");
+    pc.printf("\r\n");
 }
\ No newline at end of file