solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: servo.cpp
- Revision:
- 57:923e3df16159
- Parent:
- 56:888379912f81
- Child:
- 61:c05353850017
diff -r 888379912f81 -r 923e3df16159 servo.cpp --- a/servo.cpp Mon May 31 18:59:36 2021 +0000 +++ b/servo.cpp Tue Jun 01 07:30:06 2021 +0000 @@ -11,11 +11,37 @@ rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input } } - pitchPID.setGain(6.36, 10.6,0.0); - pitchratePID.setGain(0.9540,0.0,0.0); + + // 自身の位置に応じてエレベータ舵角を決定する + float deobj = 0.0f; + switch(pos_tail) + { + case 0: //left + deobj = -rc[1]; + break; + case 1: //center + deobj = -rc[1]; + break; + case 2: //right + deobj = -rc[1]; + break; + default: // error situation + deobj = 0.0f; + break; + } + //指令角の最大最小値をrc7で変更 + float objgain = ((rc[6]+1.0f)/2*10.0f+1.0f)*M_PI/180.0f; + float pitchobj = objgain * deobj; + + //ゲインの係数をrc8で変更 + float gaincoef = ((rc[7]+1.0f)/2*0.9)+0.1; + pitchPID.setGain(6.36*gaincoef, 10.6*gaincoef,0.0); + pitchratePID.setGain(0.9540*gaincoef,0.0,0.0); pitchPID.setProcessValue(rpy.y); pitchratePID.setProcessValue(gyro.y); - float de = pitchPID.compute()+pitchratePID.compute()-rc[1]; + //目標値のセット + pitchPID.setSetPoint(pitchobj); //目標の設定 + float de = pitchPID.compute()+pitchratePID.compute(); scaledServoOut[0]=de;