Diff: servo.cpp

Revision:

74:af3b7cedff56

Parent:

73:be7a8b8188de

Child:

75:7fa924da9d32

--- a/servo.cpp	Wed Aug 25 07:01:12 2021 +0000
+++ b/servo.cpp	Thu Aug 26 04:27:01 2021 +0000
@@ -32,24 +32,28 @@
         deobj = 0.0f;
         break;
     }
-    //指令角の最大最小値をrc7で変更
-    float objgain = ((rc[6]+1.0f)/2*10.0f+1.0f)*M_PI/180.0f;
-    float pitchobj = objgain * deobj;
+    
+    
+    float objgain =  15.0f*M_PI/180.0f;
+    float pitchobj = objgain * (deobj+updateValues.de_command);
     
     //ゲインの係数をrc8で変更
-    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);
+    float gaincoef = (rc[7]+1.0f)/2.0f;
+    if(gaincoef<0.0f){gaincoef = 0.0f;}
+    if(gaincoef>1.0f){gaincoef = 1.0f;}
+    pitchPID.setGain(6.36, 10.6*gaincoef,0.0);
+    pitchratePID.setGain(0.9540,0.0,0.0);
     pitchPID.setProcessValue(rpy.x);
     pitchratePID.setProcessValue(gyro.x);
     //目標値のセット
     pitchPID.setSetPoint(pitchobj); //目標の設定
     
     de = pitchPID.compute()+pitchratePID.compute();
+    if(de<-1.0f){de = -1.0f;}
+    if(de>1.0f){de = 1.0f;}
     
     if (pos_tail == 0)
     {
-        
         scaledServoOut[0]=-de;
     }
     else