Diff: servo.cpp

Revision:

108:e582f8bd4729

Parent:

107:78e6f7bee68e

Child:

109:27ae949bc38e

diff -r 78e6f7bee68e -r e582f8bd4729 servo.cpp
--- a/servo.cpp	Mon Mar 07 09:16:40 2022 +0000
+++ b/servo.cpp	Tue Mar 08 04:06:35 2022 +0000
@@ -54,13 +54,14 @@
     
     
     float objgain =  15.0f*M_PI/180.0f;
-    float pitchobj = objgain * (deobj+updateValues.de_command);
+    float pitchobj = -objgain * (deobj+updateValues.de_command);
+    //pc.printf("%f %f %f\r\n",pitchobj,deobj,updateValues.de_command);
     //ゲインの係数をrc8で変更
-    float gaincoef = 1.0f;
+    //float gaincoef = 1.0f;
     pitchPID.setGain(6.36f, 10.6f*0.0f,0.0);
     pitchratePID.setGain(0.9540f,0.0f,0.0f);
-    pitchPID.setProcessValue(-rpy.y);
-    pitchratePID.setProcessValue(-(gyro.y-gyroBias(2,1)));
+    pitchPID.setProcessValue(rpy.y);
+    pitchratePID.setProcessValue((gyro.y-gyroBias(2,1)));
     yawratePID.setProcessValue(gyro.z-gyroBias(3,1));
     //目標値のセット
     pitchPID.setSetPoint(pitchobj); //目標の設定
@@ -68,15 +69,16 @@
     de = pitchPID.compute()+pitchratePID.compute();
     if(de<-1.0f){de = -1.0f;}
     if(de>1.0f){de = 1.0f;}
+    pc.printf("%f %f %f %f %f\r\n",pitchobj,deobj,de,drobj,dr);
     
     dr = yawratePID.compute()+drobj;
     if(dr<-1.0f){dr = -1.0f;}
     if(dr>1.0f){dr = 1.0f;}
 
-    scaledServoOut[0]=de;
+    scaledServoOut[0]=-de;
     scaledServoOut[1]=dr;
-    scaledMotorOut[0]=rc[2]+drobj*0.3f;
-    scaledMotorOut[1]=rc[2]-drobj*0.3f;
+    scaledMotorOut[0]=rc[2]+drobj*0.2f;
+    scaledMotorOut[1]=rc[2]-drobj*0.2f;
     
     float LP_servo = 0.2;
     for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++)