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
Diff: servo.cpp
- Revision:
- 83:e69ab831031c
- Parent:
- 82:c183c29d2427
- Child:
- 85:bc03f862a316
--- a/servo.cpp Thu Sep 16 09:30:21 2021 +0000 +++ b/servo.cpp Wed Sep 22 09:30:45 2021 +0000 @@ -3,15 +3,9 @@ // 割り込まれた時点での出力(computeの結果)を返す関数 void calcServoOut() { - if(serialControlSource){ - for(int i = 0;i<4;i++){ - rc[i] = float(vp.actData[i])/1000.0f; - } - }else{ - // 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 - } + // 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 } //rc[1] = joystick.getX(); @@ -19,19 +13,19 @@ // 自身の位置に応じてエレベータ舵角を決定する - float rollObj = 20.0f*M_PI/180.0f*rc[0]; - float gaincoef = 1.0f; - rollPID.setGain(5.0*gaincoef,0.0f,0.0); - rollratePID.setGain(0.5*gaincoef,0.0f,0.0); + //float rollObj = 20.0f*M_PI/180.0f*rc[0]; + //float gaincoef = 1.0f; + //rollPID.setGain(5.0*gaincoef,0.0f,0.0); + //rollratePID.setGain(0.5*gaincoef,0.0f,0.0); pitchPID.setProcessValue(rpy.x); pitchratePID.setProcessValue(gyro.x); rollPID.setProcessValue(rpy.y); - rollPID.setSetPoint(rollObj); + //rollPID.setSetPoint(rollObj); rollratePID.setProcessValue(gyro.y); - //de = -rc[1]; + //de = rc[1]; //da = +rc[0]; - de = -(pitchPID.compute()+pitchratePID.compute()-rc[1]); - da = rollPID.compute()+rollratePID.compute(); + de = -(pitchPID.compute()+pitchratePID.compute())+rc[1]; + da = (rollPID.compute()+rollratePID.compute())+rc[0]; //float de = pitchPID.compute()+pitchratePID.compute()-rc[1]; //float da = rollPID.compute()+rollratePID.compute()+rc[0]; dT = rc[2]; @@ -68,7 +62,7 @@ servoLeft.pulsewidth_us(servoOut[1]); servoThrust.pulsewidth_us(motorOut[0]); - if(loop_count >= 10) + if(loop_count >= 3) { writeSdcard(); loop_count = 1;