Diff: servo.cpp

Revision:

92:00460f6df439

Parent:

90:96c2b0ed4b96

Child:

94:579e875a4244

--- a/servo.cpp	Tue Oct 26 05:37:25 2021 +0000
+++ b/servo.cpp	Thu Oct 28 09:44:47 2021 +0000
@@ -16,21 +16,24 @@
     rpy.z = euler(3,1);
 
     //PIDへの状態量のセット
-    pitchPID.setProcessValue(rpy.x);
-    pitchratePID.setProcessValue(gyro.x);
-    rollPID.setProcessValue(rpy.y);
-    rollratePID.setProcessValue(gyro.y);
+    pitchPID.setProcessValue(rpy.y);
+    pitchratePID.setProcessValue(gyro.y);
+    rollPID.setProcessValue(rpy.x);
+    rollratePID.setProcessValue(gyro.x);
     
     //舵角計算
-    //de = rc[1];
-    //da = rc[0];
-    de = -(pitchPID.compute()+pitchratePID.compute())+rc[1];
-    da = (rollPID.compute()+rollratePID.compute())+rc[0];
+    if(rc[4]<-0.3f){
+        de = (rc[0]-rc[1])/2.0f;
+        da = (rc[0]+rc[1])/2.0f;
+    }else{
+        de = (pitchPID.compute()+pitchratePID.compute())+(rc[0]-rc[1])/2.0f;
+        da = (rollPID.compute()+rollratePID.compute())+(rc[0]+rc[1])/2.0f;
+    }
 
     dT = rc[2];
     
-    scaledServoOut[0]=-de+da;
-    scaledServoOut[1]=de+da;
+    scaledServoOut[0]=de+da;
+    scaledServoOut[1]=-de+da;
     scaledMotorOut[0]= dT;
     
     float LP_servo = 0.2;