NHK2017 octopus robot
Dependencies: 2017NHKpin_config mbed FEP ikarashiMDC PID jy901 omni HMC6352 omni_wheel
Fork of KANI2017v2 by
bot/PIDcontroller/PID_controller.cpp@22:682cc376da6f, 2017-09-07 (annotated)
- Committer:
- number_key
- Date:
- Thu Sep 07 06:26:53 2017 +0000
- Revision:
- 22:682cc376da6f
- Parent:
- 20:477c5d039e93
- Child:
- 23:37bb9afe9fdc
save
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
UCHITAKE | 6:590c9622ecf1 | 1 | #include "PID_controller.h" |
UCHITAKE | 6:590c9622ecf1 | 2 | |
UCHITAKE | 6:590c9622ecf1 | 3 | void PIDC::updateOutput() |
UCHITAKE | 6:590c9622ecf1 | 4 | { |
UCHITAKE | 6:590c9622ecf1 | 5 | confirm(); |
UCHITAKE | 6:590c9622ecf1 | 6 | } |
UCHITAKE | 6:590c9622ecf1 | 7 | |
number_key | 22:682cc376da6f | 8 | PIDC::PIDC() : |
number_key | 22:682cc376da6f | 9 | PID(KC, TI, TD, INTERVAL), |
number_key | 22:682cc376da6f | 10 | JY901(HMCsda, HMCscl), |
number_key | 22:682cc376da6f | 11 | axisOffSetDegree(0), |
number_key | 22:682cc376da6f | 12 | planeOffSetDegree(0), |
number_key | 22:682cc376da6f | 13 | turnOverNumber(0), |
number_key | 22:682cc376da6f | 14 | beforeDegree(0), |
number_key | 22:682cc376da6f | 15 | rawDegree(0), |
number_key | 22:682cc376da6f | 16 | calculationResult(0), |
number_key | 22:682cc376da6f | 17 | axisCurrentDegree(0), |
number_key | 22:682cc376da6f | 18 | planeCurrentDegree(0) |
UCHITAKE | 6:590c9622ecf1 | 19 | { |
uchitake |
19:41f7dd1a5ed1 | 20 | PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT); |
uchitake |
19:41f7dd1a5ed1 | 21 | PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT); |
uchitake |
19:41f7dd1a5ed1 | 22 | PID::setBias(BIAS); |
uchitake |
19:41f7dd1a5ed1 | 23 | PID::setMode(AUTO_MODE); |
uchitake |
19:41f7dd1a5ed1 | 24 | PID::setSetPoint(0.0); |
UCHITAKE | 6:590c9622ecf1 | 25 | |
number_key | 20:477c5d039e93 | 26 | //HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20); |
number_key | 20:477c5d039e93 | 27 | rawDegree = JY901::getZaxisAngle(); |
number_key | 22:682cc376da6f | 28 | beforeDegree = rawDegree; |
number_key | 22:682cc376da6f | 29 | planeOffSetDegree = rawDegree; |
number_key | 22:682cc376da6f | 30 | axisOffSetDegree = rawDegree; |
UCHITAKE | 6:590c9622ecf1 | 31 | // this -> attach(this, &PIDC::updateOutput, INTERVAL); |
UCHITAKE | 6:590c9622ecf1 | 32 | } |
UCHITAKE | 6:590c9622ecf1 | 33 | |
uchitake |
19:41f7dd1a5ed1 | 34 | PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) : |
number_key | 22:682cc376da6f | 35 | PID(kc, ti, td, interval), |
number_key | 22:682cc376da6f | 36 | JY901(sda, scl), |
number_key | 22:682cc376da6f | 37 | axisOffSetDegree(0), |
number_key | 22:682cc376da6f | 38 | planeOffSetDegree(0), |
number_key | 22:682cc376da6f | 39 | turnOverNumber(0), |
number_key | 22:682cc376da6f | 40 | beforeDegree(0), |
number_key | 22:682cc376da6f | 41 | rawDegree(0), |
number_key | 22:682cc376da6f | 42 | calculationResult(0), |
number_key | 22:682cc376da6f | 43 | axisCurrentDegree(0), |
number_key | 22:682cc376da6f | 44 | planeCurrentDegree(0) |
number_key | 20:477c5d039e93 | 45 | { |
number_key | 20:477c5d039e93 | 46 | PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT); |
number_key | 20:477c5d039e93 | 47 | PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT); |
number_key | 20:477c5d039e93 | 48 | PID::setBias(BIAS); |
number_key | 20:477c5d039e93 | 49 | PID::setMode(AUTO_MODE); |
number_key | 20:477c5d039e93 | 50 | PID::setSetPoint(0.0); |
number_key | 20:477c5d039e93 | 51 | |
number_key | 20:477c5d039e93 | 52 | //HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20); |
number_key | 22:682cc376da6f | 53 | beforeDegree = rawDegree; |
number_key | 22:682cc376da6f | 54 | planeOffSetDegree = rawDegree; |
number_key | 22:682cc376da6f | 55 | axisOffSetDegree = rawDegree; |
number_key | 20:477c5d039e93 | 56 | // this -> attach(this, &PIDC::updateOutput, INTERVAL); |
number_key | 20:477c5d039e93 | 57 | } |
uchitake |
19:41f7dd1a5ed1 | 58 | |
uchitake |
19:41f7dd1a5ed1 | 59 | |
UCHITAKE | 6:590c9622ecf1 | 60 | void PIDC::confirm() |
UCHITAKE | 6:590c9622ecf1 | 61 | { |
number_key | 22:682cc376da6f | 62 | rawDegree = JY901::getZaxisAngle(); |
number_key | 22:682cc376da6f | 63 | if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber; |
number_key | 22:682cc376da6f | 64 | if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber; |
number_key | 22:682cc376da6f | 65 | axisCurrentDegree = rawDegree - axisOffSetDegree + turnOverNumber * 360; |
number_key | 22:682cc376da6f | 66 | planeCurrentDegree = rawDegree - planeOffSetDegree + turnOverNumber * 360; |
number_key | 20:477c5d039e93 | 67 | beforeDegree = JY901::getZaxisAngle(); |
number_key | 22:682cc376da6f | 68 | PID::setProcessValue(axisCurrentDegree); |
number_key | 22:682cc376da6f | 69 | calculationResult = PID::compute(); |
number_key | 22:682cc376da6f | 70 | } |
number_key | 22:682cc376da6f | 71 | |
number_key | 22:682cc376da6f | 72 | float PIDC::getCalculationResult() const |
number_key | 22:682cc376da6f | 73 | { |
number_key | 22:682cc376da6f | 74 | return calculationResult; |
UCHITAKE | 6:590c9622ecf1 | 75 | } |
UCHITAKE | 6:590c9622ecf1 | 76 | |
number_key | 22:682cc376da6f | 77 | float PIDC::getCurrentDegree() const |
number_key | 22:682cc376da6f | 78 | { |
number_key | 22:682cc376da6f | 79 | return planeCurrentDegree; |
number_key | 22:682cc376da6f | 80 | } |
number_key | 22:682cc376da6f | 81 | |
number_key | 22:682cc376da6f | 82 | void PIDC::resetAxisOffset() |
UCHITAKE | 6:590c9622ecf1 | 83 | { |
number_key | 22:682cc376da6f | 84 | beforeDegree = JY901::getZaxisAngle(); |
number_key | 22:682cc376da6f | 85 | axisOffSetDegree = JY901::getZaxisAngle(); |
number_key | 22:682cc376da6f | 86 | turnOverNumber = 0; |
uchitake |
19:41f7dd1a5ed1 | 87 | } |
number_key | 22:682cc376da6f | 88 | |
number_key | 22:682cc376da6f | 89 | void PIDC::resetPlaneOffset() |
number_key | 22:682cc376da6f | 90 | { |
number_key | 22:682cc376da6f | 91 | beforeDegree = JY901::getZaxisAngle(); |
number_key | 22:682cc376da6f | 92 | planeOffSetDegree = JY901::getZaxisAngle(); |
number_key | 22:682cc376da6f | 93 | turnOverNumber = 0; |
number_key | 22:682cc376da6f | 94 | } |
number_key | 22:682cc376da6f | 95 | |
number_key | 22:682cc376da6f | 96 | void PIDC::calibration(int mode) |
number_key | 22:682cc376da6f | 97 | { |
number_key | 22:682cc376da6f | 98 | //setCalibrationMode(mode); |
number_key | 22:682cc376da6f | 99 | } |