Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
servo.cpp
- Committer:
- NaotoMorita
- Date:
- 2021-06-22
- Revision:
- 66:e5afad70fdd8
- Parent:
- 62:ef10fd919f7b
- Child:
- 68:b9f6938fab9d
File content as of revision 66:e5afad70fdd8:
#include "global.hpp" // 割り込まれた時点での出力(computeの結果)を返す関数 void calcServoOut() { // 自身の位置に応じてエレベータ舵角を決定する 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; float LP_servo = 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; } } elevServo.pulsewidth_us(servoOut[0]); rudServo.pulsewidth_us(servoOut[1]); if(loop_count >= 10) { writeSdcard(); loop_count = 1; } else { loop_count +=1; } }