NHK2017 octopus robot
Dependencies: 2017NHKpin_config mbed FEP ikarashiMDC PID jy901 omni HMC6352 omni_wheel
Fork of KANI2017v2 by
bot/PIDcontroller/PID_controller.cpp@23:37bb9afe9fdc, 2017-09-13 (annotated)
- Committer:
- number_key
- Date:
- Wed Sep 13 14:26:47 2017 +0900
- Revision:
- 23:37bb9afe9fdc
- Parent:
- 22:682cc376da6f
- Child:
- 26:7258d5ad0bff
move
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 | rawDegree = JY901::getZaxisAngle(); |
number_key | 22:682cc376da6f | 27 | beforeDegree = rawDegree; |
number_key | 22:682cc376da6f | 28 | planeOffSetDegree = rawDegree; |
number_key | 22:682cc376da6f | 29 | axisOffSetDegree = rawDegree; |
UCHITAKE | 6:590c9622ecf1 | 30 | // this -> attach(this, &PIDC::updateOutput, INTERVAL); |
UCHITAKE | 6:590c9622ecf1 | 31 | } |
UCHITAKE | 6:590c9622ecf1 | 32 | |
uchitake |
19:41f7dd1a5ed1 | 33 | PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) : |
number_key | 22:682cc376da6f | 34 | PID(kc, ti, td, interval), |
number_key | 22:682cc376da6f | 35 | JY901(sda, scl), |
number_key | 22:682cc376da6f | 36 | axisOffSetDegree(0), |
number_key | 22:682cc376da6f | 37 | planeOffSetDegree(0), |
number_key | 22:682cc376da6f | 38 | turnOverNumber(0), |
number_key | 22:682cc376da6f | 39 | beforeDegree(0), |
number_key | 22:682cc376da6f | 40 | rawDegree(0), |
number_key | 22:682cc376da6f | 41 | calculationResult(0), |
number_key | 22:682cc376da6f | 42 | axisCurrentDegree(0), |
number_key | 22:682cc376da6f | 43 | planeCurrentDegree(0) |
number_key | 20:477c5d039e93 | 44 | { |
number_key | 20:477c5d039e93 | 45 | PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT); |
number_key | 20:477c5d039e93 | 46 | PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT); |
number_key | 20:477c5d039e93 | 47 | PID::setBias(BIAS); |
number_key | 20:477c5d039e93 | 48 | PID::setMode(AUTO_MODE); |
number_key | 20:477c5d039e93 | 49 | PID::setSetPoint(0.0); |
number_key | 20:477c5d039e93 | 50 | |
number_key |
23:37bb9afe9fdc | 51 | rawDegree = JY901::getZaxisAngle(); |
number_key | 22:682cc376da6f | 52 | beforeDegree = rawDegree; |
number_key | 22:682cc376da6f | 53 | planeOffSetDegree = rawDegree; |
number_key | 22:682cc376da6f | 54 | axisOffSetDegree = rawDegree; |
number_key | 20:477c5d039e93 | 55 | // this -> attach(this, &PIDC::updateOutput, INTERVAL); |
number_key | 20:477c5d039e93 | 56 | } |
uchitake |
19:41f7dd1a5ed1 | 57 | |
uchitake |
19:41f7dd1a5ed1 | 58 | |
UCHITAKE | 6:590c9622ecf1 | 59 | void PIDC::confirm() |
UCHITAKE | 6:590c9622ecf1 | 60 | { |
number_key | 22:682cc376da6f | 61 | rawDegree = JY901::getZaxisAngle(); |
number_key | 22:682cc376da6f | 62 | if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber; |
number_key | 22:682cc376da6f | 63 | if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber; |
number_key | 22:682cc376da6f | 64 | axisCurrentDegree = rawDegree - axisOffSetDegree + turnOverNumber * 360; |
number_key | 22:682cc376da6f | 65 | planeCurrentDegree = rawDegree - planeOffSetDegree + turnOverNumber * 360; |
number_key | 20:477c5d039e93 | 66 | beforeDegree = JY901::getZaxisAngle(); |
number_key | 22:682cc376da6f | 67 | PID::setProcessValue(axisCurrentDegree); |
number_key | 22:682cc376da6f | 68 | calculationResult = PID::compute(); |
number_key | 22:682cc376da6f | 69 | } |
number_key | 22:682cc376da6f | 70 | |
number_key | 22:682cc376da6f | 71 | float PIDC::getCalculationResult() const |
number_key | 22:682cc376da6f | 72 | { |
number_key | 22:682cc376da6f | 73 | return calculationResult; |
UCHITAKE | 6:590c9622ecf1 | 74 | } |
UCHITAKE | 6:590c9622ecf1 | 75 | |
number_key | 22:682cc376da6f | 76 | float PIDC::getCurrentDegree() const |
number_key | 22:682cc376da6f | 77 | { |
number_key | 22:682cc376da6f | 78 | return planeCurrentDegree; |
number_key | 22:682cc376da6f | 79 | } |
number_key | 22:682cc376da6f | 80 | |
number_key | 22:682cc376da6f | 81 | void PIDC::resetAxisOffset() |
UCHITAKE | 6:590c9622ecf1 | 82 | { |
number_key | 22:682cc376da6f | 83 | beforeDegree = JY901::getZaxisAngle(); |
number_key | 22:682cc376da6f | 84 | axisOffSetDegree = JY901::getZaxisAngle(); |
number_key | 22:682cc376da6f | 85 | turnOverNumber = 0; |
uchitake |
19:41f7dd1a5ed1 | 86 | } |
number_key | 22:682cc376da6f | 87 | |
number_key | 22:682cc376da6f | 88 | void PIDC::resetPlaneOffset() |
number_key | 22:682cc376da6f | 89 | { |
number_key | 22:682cc376da6f | 90 | beforeDegree = JY901::getZaxisAngle(); |
number_key | 22:682cc376da6f | 91 | planeOffSetDegree = JY901::getZaxisAngle(); |
number_key | 22:682cc376da6f | 92 | turnOverNumber = 0; |
number_key | 22:682cc376da6f | 93 | } |
number_key | 22:682cc376da6f | 94 | |
number_key |
23:37bb9afe9fdc | 95 | void PIDC::renewAngle() |
number_key |
23:37bb9afe9fdc | 96 | { |
number_key |
23:37bb9afe9fdc | 97 | angle = JY901::getZaxisAngle(); |
number_key |
23:37bb9afe9fdc | 98 | } |
number_key |
23:37bb9afe9fdc | 99 | |
number_key | 22:682cc376da6f | 100 | void PIDC::calibration(int mode) |
number_key | 22:682cc376da6f | 101 | { |
number_key |
23:37bb9afe9fdc | 102 | |
number_key | 22:682cc376da6f | 103 | } |