Diff: servo.cpp

Revision:

139:b378528c05f2

Parent:

137:240588882ae2

Child:

140:53dbdb207542

--- a/servo.cpp	Wed Dec 01 19:17:36 2021 +0000
+++ b/servo.cpp	Mon Dec 06 08:26:16 2021 +0000
@@ -1,93 +1,93 @@
-#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.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