Diff: servo.cpp

Revision:

140:53dbdb207542

Parent:

139:b378528c05f2

Child:

141:725321fe2949

--- a/servo.cpp	Mon Dec 06 08:26:16 2021 +0000
+++ b/servo.cpp	Mon Dec 06 11:37:55 2021 +0000
@@ -1,93 +1,91 @@
-//#include "global.hpp"
-//
-//// 割り込まれた時点での出力(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
-//    }
-//
-//    
-//    //姿勢角の所得
-//    euler = eskf.computeAngles();
-//    rpy.x = euler(1,1);
-//    rpy.y = euler(2,1);
-//    rpy.z = euler(3,1);
-//
-//    //PIDへの状態量のセット
-//    pitchPID.setProcessValue(rpy.y);
-//    pitchratePID.setProcessValue(gyro.y);
-//    rollPID.setProcessValue(rpy.x);
-//    rollratePID.setProcessValue(gyro.x);
-//    
-//    dT = rc[2];
-//    
-//    if (rc[4]>-0.3f && rc[6] < -0.3f)
-//    {
-//        //level_flight();
-//        //point_guide();
-//        climb();
-//        rollPID.setSetPoint(roll_obj);
-//        pitchPID.setSetPoint(pitch_obj);
-//        dT += dT_obj;
-//    }else{
-//        rollPID.setSetPoint(0.0f);
-//        pitchPID.setSetPoint(0.0f);
-//    }
-//    
-//    //舵角計算
-//    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;
-//    }
-//    
-//    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];
-//        if(servoOut[i]<servoPwmMin)
-//        {
-//            servoOut[i] = servoPwmMin;
-//        }
-//        if(servoOut[i]>servoPwmMax)
-//        {
-//            servoOut[i] = servoPwmMax;
-//        }
-//    }
-//    
-//    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]);
-//
-//    sendData2PC();
-//    writeSDcard();
-//    
-//    if(loop_count >= 5)
-//    {
-//        sendTelemetry();
-//        loop_count = 1;
-//
-//    }
-//    else
-//    {
-//        loop_count +=1;
-//    }
-//}
\ No newline at end of file
+#include "global.hpp"
+
+// 割り込まれた時点での出力(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
+    }
+
+    
+    //姿勢角の所得
+    euler = eskf.computeAngles();
+    rpy = euler;
+
+    //PIDへの状態量のセット
+    pitchPID.setProcessValue(rpy(1));
+    pitchratePID.setProcessValue(gyro(1));
+    rollPID.setProcessValue(rpy(0));
+    rollratePID.setProcessValue(gyro(0));
+    
+    dT = rc[2];
+    
+    if (rc[4]>-0.3f && rc[6] < -0.3f)
+    {
+        //level_flight();
+        //point_guide();
+        climb();
+        rollPID.setSetPoint(roll_obj);
+        pitchPID.setSetPoint(pitch_obj);
+        dT += dT_obj;
+    }else{
+        rollPID.setSetPoint(0.0f);
+        pitchPID.setSetPoint(0.0f);
+    }
+    
+    //舵角計算
+    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;
+    }
+    
+    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.0f,1.0f,servoPwmMin,servoPwmMax))+(1.0f-LP_servo)*servoOut[i];
+        if(servoOut[i]<servoPwmMin)
+        {
+            servoOut[i] = servoPwmMin;
+        }
+        if(servoOut[i]>servoPwmMax)
+        {
+            servoOut[i] = servoPwmMax;
+        }
+    }
+    
+    for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){
+        motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1.0f,1.0f,motorPwmMin,motorPwmMax))+(1.0f-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]);
+
+    sendData2PC();
+    writeSDcard();
+    
+    if(loop_count >= 5)
+    {
+        sendTelemetry();
+        loop_count = 1;
+
+    }
+    else
+    {
+        loop_count +=1;
+    }
+}
\ No newline at end of file