NHK2017Ateamかにろぼ
Dependencies: 2017NHKpin_config mbed FEP HMC6352 MotorDriverController PID QEI omni
\ ̄\ / ̄/ /l \ \ / / lヽ | ヽ ヽ | | / / | \ ` ‐ヽ ヽ ● ● / / ‐ / \ __ l | ||___|| / l __ / \ \ / \/ /\| 人__人 |/\ //\| |/\\ //\| |/\\ / . \_____/ \ ┏┓ ┏━┓┏┓ ┏┓ ┏┓┏┓ ┏┓ ┏┓┗┛ ┏┓ ┗┓┃┗┛ ┏┛┗━┓ ┃┃┃┃ ┃┃┏━┛┗┓┏┓┏┛┗━┓┃┃┏┓┏┓┏━━━┓ ┗┓┏━┛ ┃┃┗┛ ┃┃┗━┓┏┛┗┛┗┓┏┓┃┗┛┗┛┃┃┗━━━┛ ┏┛┃┏━┓┃┗━━┓┃┃┏━┛┗┓ ┏┛┃┃┃ ┃┃ ┃┏┛┗━┛┗━━┓┃┃┃┃┏┓┏┛ ┗━┛┃┃ ┃┃┏┓ ┃┃┏━━┓┏━━┛┃┃┃┃┗┛┃ ┏┛┃ ┃┃┃┗━━┓ ┗┛┗━━┛┗━━━┛┗┛┗━━┛ ┗━┛ ┗┛┗━━━┛
bot/PIDcontroller/PID_controller.cpp@23:98746361f827, 2017-09-05 (annotated)
- Committer:
- uchitake
- Date:
- Tue Sep 05 12:02:47 2017 +0900
- Revision:
- 23:98746361f827
- Parent:
- 22:bb6afe7332c3
remove WheelUnit
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 | |
uchitake |
22:bb6afe7332c3 | 8 | PIDC::PIDC() : |
uchitake |
22:bb6afe7332c3 | 9 | PID(KC, TI, TD, INTERVAL), |
uchitake |
22:bb6afe7332c3 | 10 | HMC6352(HMCsda, HMCscl), |
uchitake |
22:bb6afe7332c3 | 11 | rawDegree(0), |
uchitake |
22:bb6afe7332c3 | 12 | offSetDegree(0), |
uchitake |
22:bb6afe7332c3 | 13 | turnOverNumber(0), |
uchitake |
22:bb6afe7332c3 | 14 | beforeDegree(0), |
uchitake |
22:bb6afe7332c3 | 15 | co(0), |
uchitake |
22:bb6afe7332c3 | 16 | processValue(0), |
uchitake |
22:bb6afe7332c3 | 17 | initDegree(0) |
UCHITAKE | 6:590c9622ecf1 | 18 | { |
uchitake |
18:41f7dd1a5ed1 | 19 | PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT); |
uchitake |
18:41f7dd1a5ed1 | 20 | PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT); |
uchitake |
18:41f7dd1a5ed1 | 21 | PID::setBias(BIAS); |
uchitake |
18:41f7dd1a5ed1 | 22 | PID::setMode(AUTO_MODE); |
uchitake |
18:41f7dd1a5ed1 | 23 | PID::setSetPoint(0.0); |
UCHITAKE | 6:590c9622ecf1 | 24 | |
uchitake |
18:41f7dd1a5ed1 | 25 | HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20); |
uchitake |
18:41f7dd1a5ed1 | 26 | rawDegree = HMC6352::sample(); |
uchitake |
18:41f7dd1a5ed1 | 27 | beforeDegree = HMC6352::sample(); |
uchitake |
18:41f7dd1a5ed1 | 28 | offSetDegree = HMC6352::sample(); |
UCHITAKE | 6:590c9622ecf1 | 29 | initDegree = 0; |
UCHITAKE | 6:590c9622ecf1 | 30 | turnOverNumber = 0; |
UCHITAKE | 6:590c9622ecf1 | 31 | // this -> attach(this, &PIDC::updateOutput, INTERVAL); |
UCHITAKE | 6:590c9622ecf1 | 32 | } |
UCHITAKE | 6:590c9622ecf1 | 33 | |
uchitake |
18:41f7dd1a5ed1 | 34 | PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) : |
uchitake |
22:bb6afe7332c3 | 35 | PID(kc, ti, td, interval), |
uchitake |
22:bb6afe7332c3 | 36 | HMC6352(sda, scl), |
uchitake |
22:bb6afe7332c3 | 37 | rawDegree(0), |
uchitake |
22:bb6afe7332c3 | 38 | offSetDegree(0), |
uchitake |
22:bb6afe7332c3 | 39 | turnOverNumber(0), |
uchitake |
22:bb6afe7332c3 | 40 | beforeDegree(0), |
uchitake |
22:bb6afe7332c3 | 41 | co(0), |
uchitake |
22:bb6afe7332c3 | 42 | processValue(0), |
uchitake |
22:bb6afe7332c3 | 43 | initDegree(0) |
UCHITAKE | 19:3a62cbc6fee9 | 44 | { |
UCHITAKE | 19:3a62cbc6fee9 | 45 | PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT); |
UCHITAKE | 19:3a62cbc6fee9 | 46 | PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT); |
UCHITAKE | 19:3a62cbc6fee9 | 47 | PID::setBias(BIAS); |
UCHITAKE | 19:3a62cbc6fee9 | 48 | PID::setMode(AUTO_MODE); |
UCHITAKE | 19:3a62cbc6fee9 | 49 | PID::setSetPoint(0.0); |
UCHITAKE | 19:3a62cbc6fee9 | 50 | |
UCHITAKE | 19:3a62cbc6fee9 | 51 | HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20); |
UCHITAKE | 19:3a62cbc6fee9 | 52 | rawDegree = HMC6352::sample(); |
UCHITAKE | 19:3a62cbc6fee9 | 53 | beforeDegree = HMC6352::sample(); |
UCHITAKE | 19:3a62cbc6fee9 | 54 | offSetDegree = HMC6352::sample(); |
UCHITAKE | 19:3a62cbc6fee9 | 55 | initDegree = 0; |
UCHITAKE | 19:3a62cbc6fee9 | 56 | turnOverNumber = 0; |
UCHITAKE | 19:3a62cbc6fee9 | 57 | // this -> attach(this, &PIDC::updateOutput, INTERVAL); |
UCHITAKE | 19:3a62cbc6fee9 | 58 | } |
uchitake |
18:41f7dd1a5ed1 | 59 | |
uchitake |
18:41f7dd1a5ed1 | 60 | |
UCHITAKE | 6:590c9622ecf1 | 61 | void PIDC::confirm() |
UCHITAKE | 6:590c9622ecf1 | 62 | { |
uchitake |
18:41f7dd1a5ed1 | 63 | rawDegree = HMC6352::sample(); |
UCHITAKE | 6:590c9622ecf1 | 64 | if(rawDegree - beforeDegree < -1800) turnOverNumber++; |
UCHITAKE | 6:590c9622ecf1 | 65 | if(rawDegree - beforeDegree > 1800) turnOverNumber--; |
UCHITAKE | 6:590c9622ecf1 | 66 | initDegree = rawDegree - offSetDegree + turnOverNumber * 3600; |
uchitake |
18:41f7dd1a5ed1 | 67 | beforeDegree = HMC6352::sample(); |
uchitake |
18:41f7dd1a5ed1 | 68 | PID::setProcessValue(initDegree / 10.0); |
uchitake |
18:41f7dd1a5ed1 | 69 | co = PID::compute(); |
UCHITAKE | 6:590c9622ecf1 | 70 | } |
UCHITAKE | 6:590c9622ecf1 | 71 | |
uchitake |
22:bb6afe7332c3 | 72 | float PIDC::getCo() const |
UCHITAKE | 6:590c9622ecf1 | 73 | { |
UCHITAKE | 6:590c9622ecf1 | 74 | return co; |
uchitake |
18:41f7dd1a5ed1 | 75 | } |
UCHITAKE | 19:3a62cbc6fee9 | 76 | |
UCHITAKE | 19:3a62cbc6fee9 | 77 | |
UCHITAKE | 19:3a62cbc6fee9 | 78 | void PIDC::calibration(int mode) |
UCHITAKE | 19:3a62cbc6fee9 | 79 | { |
UCHITAKE | 19:3a62cbc6fee9 | 80 | setCalibrationMode(mode); |
UCHITAKE | 19:3a62cbc6fee9 | 81 | } |