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:
- 92:00460f6df439
- Parent:
- 90:96c2b0ed4b96
- Child:
- 94:579e875a4244
--- a/servo.cpp Tue Oct 26 05:37:25 2021 +0000 +++ b/servo.cpp Thu Oct 28 09:44:47 2021 +0000 @@ -16,21 +16,24 @@ rpy.z = euler(3,1); //PIDへの状態量のセット - pitchPID.setProcessValue(rpy.x); - pitchratePID.setProcessValue(gyro.x); - rollPID.setProcessValue(rpy.y); - rollratePID.setProcessValue(gyro.y); + pitchPID.setProcessValue(rpy.y); + pitchratePID.setProcessValue(gyro.y); + rollPID.setProcessValue(rpy.x); + rollratePID.setProcessValue(gyro.x); //舵角計算 - //de = rc[1]; - //da = rc[0]; - de = -(pitchPID.compute()+pitchratePID.compute())+rc[1]; - da = (rollPID.compute()+rollratePID.compute())+rc[0]; + 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; + } dT = rc[2]; - scaledServoOut[0]=-de+da; - scaledServoOut[1]=de+da; + scaledServoOut[0]=de+da; + scaledServoOut[1]=-de+da; scaledMotorOut[0]= dT; float LP_servo = 0.2;