Diff: servo.cpp

Revision:

107:78e6f7bee68e

Parent:

88:0fc5df2fddcb

Child:

108:e582f8bd4729

diff -r 36458fb9b5b7 -r 78e6f7bee68e servo.cpp
--- a/servo.cpp	Wed Mar 02 09:23:39 2022 +0000
+++ b/servo.cpp	Mon Mar 07 09:16:40 2022 +0000
@@ -15,6 +15,7 @@
     
     //姿勢角の取得
     euler = eskf.computeAngles();
+    Matrix gyroBias = eskf.getGyroBias();
     rpy.x = euler(1,1);
     rpy.y = euler(2,1);
     rpy.z = euler(3,1);
@@ -49,25 +50,33 @@
         break;
     }
     
+    drobj = rc[3];
+    
     
     float objgain =  15.0f*M_PI/180.0f;
     float pitchobj = objgain * (deobj+updateValues.de_command);
     //ゲインの係数をrc8で変更
     float gaincoef = 1.0f;
-    if(gaincoef<0.0f){gaincoef = 0.0f;}
-    if(gaincoef>1.0f){gaincoef = 1.0f;}
-    pitchPID.setGain(6.36, 10.6*gaincoef*0.0f,0.0);
-    pitchratePID.setGain(0.9540,0.0,0.0);
+    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);
+    pitchratePID.setProcessValue(-(gyro.y-gyroBias(2,1)));
+    yawratePID.setProcessValue(gyro.z-gyroBias(3,1));
     //目標値のセット
     pitchPID.setSetPoint(pitchobj); //目標の設定
     
     de = pitchPID.compute()+pitchratePID.compute();
     if(de<-1.0f){de = -1.0f;}
     if(de>1.0f){de = 1.0f;}
+    
+    dr = yawratePID.compute()+drobj;
+    if(dr<-1.0f){dr = -1.0f;}
+    if(dr>1.0f){dr = 1.0f;}
 
     scaledServoOut[0]=de;
+    scaledServoOut[1]=dr;
+    scaledMotorOut[0]=rc[2]+drobj*0.3f;
+    scaledMotorOut[1]=rc[2]-drobj*0.3f;
     
     float LP_servo = 0.2;
     for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++)
@@ -83,6 +92,23 @@
         }
     }
     
+    float LP_motor = 0.2;
+    for(int i = 0; i < sizeof(motorOut)/sizeof(motorOut[0]); i++)
+    {
+        motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i];
+        if(motorOut[i]<motorPwmMin)
+        {
+            motorOut[i] = motorPwmMin;
+        }
+        if(motorOut[i]>motorPwmMax)
+        {
+            motorOut[i] = motorPwmMax;
+        }
+    }
+    
     servo.pulsewidth_us(servoOut[0]);
+    rudServo.pulsewidth_us(servoOut[1]);
+    motor1.pulsewidth_us(motorOut[0]);
+    motor2.pulsewidth_us(motorOut[1]);
     send2center();
 }
\ No newline at end of file