![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
NHK2017 octopus robot
Dependencies: 2017NHKpin_config mbed FEP ikarashiMDC PID jy901 omni HMC6352 omni_wheel
Fork of KANI2017v2 by
Diff: bot/PIDcontroller/PID_controller.cpp
- Revision:
- 22:682cc376da6f
- Parent:
- 20:477c5d039e93
- Child:
- 23:37bb9afe9fdc
diff -r 9c1061982b16 -r 682cc376da6f bot/PIDcontroller/PID_controller.cpp --- a/bot/PIDcontroller/PID_controller.cpp Tue Sep 05 14:07:18 2017 +0900 +++ b/bot/PIDcontroller/PID_controller.cpp Thu Sep 07 06:26:53 2017 +0000 @@ -5,7 +5,17 @@ confirm(); } -PIDC::PIDC() : PID(KC, TI, TD, INTERVAL), JY901(HMCsda, HMCscl) +PIDC::PIDC() : + PID(KC, TI, TD, INTERVAL), + JY901(HMCsda, HMCscl), + axisOffSetDegree(0), + planeOffSetDegree(0), + turnOverNumber(0), + beforeDegree(0), + rawDegree(0), + calculationResult(0), + axisCurrentDegree(0), + planeCurrentDegree(0) { PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT); PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT); @@ -15,15 +25,23 @@ //HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20); rawDegree = JY901::getZaxisAngle(); - beforeDegree = JY901::getZaxisAngle(); - offSetDegree = JY901::getZaxisAngle(); - initDegree = 0; - turnOverNumber = 0; + beforeDegree = rawDegree; + planeOffSetDegree = rawDegree; + axisOffSetDegree = rawDegree; // this -> attach(this, &PIDC::updateOutput, INTERVAL); } PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) : -PID(kc, ti, td, interval), JY901(sda, scl) + PID(kc, ti, td, interval), + JY901(sda, scl), + axisOffSetDegree(0), + planeOffSetDegree(0), + turnOverNumber(0), + beforeDegree(0), + rawDegree(0), + calculationResult(0), + axisCurrentDegree(0), + planeCurrentDegree(0) { PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT); PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT); @@ -32,27 +50,50 @@ PID::setSetPoint(0.0); //HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20); - rawDegree = JY901::getZaxisAngle(); - beforeDegree = JY901::getZaxisAngle(); - offSetDegree = JY901::getZaxisAngle(); - initDegree = 0; - turnOverNumber = 0; + beforeDegree = rawDegree; + planeOffSetDegree = rawDegree; + axisOffSetDegree = rawDegree; // this -> attach(this, &PIDC::updateOutput, INTERVAL); } void PIDC::confirm() { - rawDegree = getZaxisAngle(); - if(rawDegree - beforeDegree < -1800) turnOverNumber++; - if(rawDegree - beforeDegree > 1800) turnOverNumber--; - initDegree = rawDegree - offSetDegree + turnOverNumber * 3600; + rawDegree = JY901::getZaxisAngle(); + if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber; + if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber; + axisCurrentDegree = rawDegree - axisOffSetDegree + turnOverNumber * 360; + planeCurrentDegree = rawDegree - planeOffSetDegree + turnOverNumber * 360; beforeDegree = JY901::getZaxisAngle(); - PID::setProcessValue(initDegree / 10.0); - co = PID::compute(); + PID::setProcessValue(axisCurrentDegree); + calculationResult = PID::compute(); +} + +float PIDC::getCalculationResult() const +{ + return calculationResult; } -float PIDC::getCo() +float PIDC::getCurrentDegree() const +{ + return planeCurrentDegree; +} + +void PIDC::resetAxisOffset() { - return co; + beforeDegree = JY901::getZaxisAngle(); + axisOffSetDegree = JY901::getZaxisAngle(); + turnOverNumber = 0; } + +void PIDC::resetPlaneOffset() +{ + beforeDegree = JY901::getZaxisAngle(); + planeOffSetDegree = JY901::getZaxisAngle(); + turnOverNumber = 0; +} + +void PIDC::calibration(int mode) +{ + //setCalibrationMode(mode); +}