solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: main.cpp
- Revision:
- 29:34ee662f454e
- Parent:
- 28:fc6c46c1db65
- Child:
- 30:214ddc613a35
diff -r fc6c46c1db65 -r 34ee662f454e main.cpp --- a/main.cpp Tue Feb 16 03:24:20 2021 +0000 +++ b/main.cpp Tue Feb 16 03:27:20 2021 +0000 @@ -90,7 +90,7 @@ scaledServoOut[1]=de-da; scaledMotorOut[0]= dT; for(int i = 0;i<sizeof(servoOut);i++){ - servoOut[i] = int(map(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax)) + servoOut[i] = int(map(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax)); if(servoOut[i]<servoPwmMin) { servoOut[i] = servoPwmMin; }; @@ -100,7 +100,7 @@ } for(int i = 0;i<sizeof(motorOut);i++){ - motorOut[i] = int(map(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax)) + motorOut[i] = int(map(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax)); if(motorOut[i]<motorPwmMin) { motorOut[i] = motorPwmMin; }; @@ -112,7 +112,7 @@ servoRight.pulsewidth_us(servoOut[0]); //servoLeft.pulsewidth_us(servoOut[1]); - if(loop_count > 10) + if(loop_count > 10){ pushto_sdcard(); loop_count = 0; }else{ @@ -161,18 +161,19 @@ pitchPID.setOutputLimits(-1.0,1.0); pitchratePID.setOutputLimits(-1.0,1.0); rollPID.setOutputLimits(-1.0,1.0); - rollratePID.setOutputLimits(-pi,pi); + rollratePID.setOutputLimits(-1.0,1.0); pitchPID.setInputLimits(-pi,pi); pitchratePID.setInputLimits(-pi,pi); rollPID.setInputLimits(-pi,pi); rollratePID.setInputLimits(-pi,pi); + servoRight.period_us(20000); MadgwickFilter.begin(100); //サンプリング周波数Hza t.start(); while(1) { tstart = t.read(); //姿勢角を更新 - updateAttitude() + updateAttitude(); PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する float tend = t.read();