Diff: servo.cpp

Revision:

70:99f974d8960e

Parent:

68:b9f6938fab9d

Child:

73:84ffa0166e6c

--- a/servo.cpp	Mon Jun 28 01:45:12 2021 +0000
+++ b/servo.cpp	Tue Jun 29 08:07:55 2021 +0000
@@ -3,27 +3,29 @@
 // 割り込まれた時点での出力(computeの結果）を返す関数
 void calcServoOut()
 {
+    // sbusデータの読み込み
+    for (int i =0 ; i < 16;i ++){
+        rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
+    }
     
     // 自身の位置に応じてエレベータ舵角を決定する
-    float deobj = 0.0f;
-    float drobj = 0.0f;
-    
-    //指令角の最大最小値をrc7で変更
-    float pitchobj = 10.0f*M_PI/180.0f * deobj;
     
-    //ゲインの係数を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);
-    pitchPID.setProcessValue(rpy.y);
-    pitchratePID.setProcessValue(gyro.y);
-    //目標値のセット
-    pitchPID.setSetPoint(pitchobj); //目標の設定
-    float de = pitchPID.compute()+pitchratePID.compute();
-    float dr = 0.0f;
-    scaledServoOut[0]=de;
-    scaledServoOut[1]=dr;
+    pitchPID.setProcessValue(rpy.x);
+    pitchratePID.setProcessValue(gyro.x);
+    rollPID.setProcessValue(rpy.y);
+    rollratePID.setProcessValue(gyro.y);
+    float de = pitchPID.compute()+pitchratePID.compute()-rc[1];
+    float da = rollPID.compute()+rollratePID.compute()+rc[0];
+    //float de = pitchPID.compute()+pitchratePID.compute()-rc[1];
+    //float da = rollPID.compute()+rollratePID.compute()+rc[0];
+    float dT = rc[2];
+    
+    scaledServoOut[0]=de+da;
+    scaledServoOut[1]=-de+da;
+    scaledMotorOut[0]= dT;
+    
     float LP_servo = 0.2;
+    float LP_motor = 0.2;
     for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++)
     {
         servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
@@ -37,9 +39,18 @@
         }
     }
     
-    elevServo.pulsewidth_us(servoOut[0]);
-    rudServo.pulsewidth_us(servoOut[1]);
- 
+    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;
+        };
+    }
+    servoRight.pulsewidth_us(servoOut[0]);
+    servoLeft.pulsewidth_us(servoOut[1]); 
+    servoThrust.pulsewidth_us(motorOut[0]);
 
     if(loop_count >= 10)
     {