Diff: servo.cpp

Revision:

83:e69ab831031c

Parent:

82:c183c29d2427

Child:

85:bc03f862a316

--- a/servo.cpp	Thu Sep 16 09:30:21 2021 +0000
+++ b/servo.cpp	Wed Sep 22 09:30:45 2021 +0000
@@ -3,15 +3,9 @@
 // 割り込まれた時点での出力(computeの結果）を返す関数
 void calcServoOut()
 {
-    if(serialControlSource){
-        for(int i = 0;i<4;i++){
-            rc[i] = float(vp.actData[i])/1000.0f;
-        }
-    }else{
-        // 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
-        }
+    // 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
     }
     
     //rc[1] = joystick.getX();
@@ -19,19 +13,19 @@
     
     // 自身の位置に応じてエレベータ舵角を決定する
     
-    float rollObj = 20.0f*M_PI/180.0f*rc[0];
-    float gaincoef = 1.0f;
-    rollPID.setGain(5.0*gaincoef,0.0f,0.0);
-    rollratePID.setGain(0.5*gaincoef,0.0f,0.0);
+    //float rollObj = 20.0f*M_PI/180.0f*rc[0];
+    //float gaincoef = 1.0f;
+    //rollPID.setGain(5.0*gaincoef,0.0f,0.0);
+    //rollratePID.setGain(0.5*gaincoef,0.0f,0.0);
     pitchPID.setProcessValue(rpy.x);
     pitchratePID.setProcessValue(gyro.x);
     rollPID.setProcessValue(rpy.y);
-    rollPID.setSetPoint(rollObj);
+    //rollPID.setSetPoint(rollObj);
     rollratePID.setProcessValue(gyro.y);
-    //de = -rc[1];
+    //de = rc[1];
     //da = +rc[0];
-    de = -(pitchPID.compute()+pitchratePID.compute()-rc[1]);
-    da = rollPID.compute()+rollratePID.compute();
+    de = -(pitchPID.compute()+pitchratePID.compute())+rc[1];
+    da = (rollPID.compute()+rollratePID.compute())+rc[0];
     //float de = pitchPID.compute()+pitchratePID.compute()-rc[1];
     //float da = rollPID.compute()+rollratePID.compute()+rc[0];
     dT = rc[2];
@@ -68,7 +62,7 @@
     servoLeft.pulsewidth_us(servoOut[1]); 
     servoThrust.pulsewidth_us(motorOut[0]);
 
-    if(loop_count >= 10)
+    if(loop_count >= 3)
     {
         writeSdcard();
         loop_count = 1;