Diff: servo.cpp

Revision:

71:62eb45ecffe9

Parent:

69:0caaad87cf1d

Child:

73:be7a8b8188de

--- a/servo.cpp	Tue Jun 29 10:53:06 2021 +0000
+++ b/servo.cpp	Fri Aug 20 07:48:41 2021 +0000
@@ -14,17 +14,19 @@
     //pc.printf("rc:%f\r\n",rc[1]);
     
     // 自身の位置に応じてエレベータ舵角を決定する
+    float derc =  rc[1];
+    float darc =  rc[0]-rc[1];
     deobj = 0.0f;
     switch(pos_tail)
     {
     case 0: //left
-        deobj = -rc[1];
+        deobj = derc-darc;
         break;
     case 1: //center
-        deobj = -rc[1];
+        deobj = derc;
         break;
     case 2: //right
-        deobj = -rc[1];
+        deobj = derc+darc;
         break;
     default:   // error situation
         deobj = 0.0f;
@@ -38,20 +40,21 @@
     float gaincoef = 1.0;
     pitchPID.setGain(6.36*gaincoef, 10.6*gaincoef*0.0f,0.0);
     pitchratePID.setGain(0.9540*gaincoef,0.0,0.0);
-    pitchPID.setProcessValue(rpy.y);
-    pitchratePID.setProcessValue(gyro.y);
+    pitchPID.setProcessValue(rpy.x);
+    pitchratePID.setProcessValue(gyro.x);
     //目標値のセット
     pitchPID.setSetPoint(pitchobj); //目標の設定
+    
     de = pitchPID.compute()+pitchratePID.compute();
     
     if (pos_tail == 0)
     {
         
-        scaledServoOut[0]=de;
+        scaledServoOut[0]=-de;
     }
     else
     {
-        scaledServoOut[0]=-de;
+        scaledServoOut[0]=de;
     }
     
     float LP_servo = 0.2;
@@ -74,7 +77,6 @@
     {
         writeSdcard();
         loop_count = 1;
-        obs_count = 0;
     }
     else
     {