NHK2017 octopus robot
Dependencies: 2017NHKpin_config mbed FEP ikarashiMDC PID jy901 omni HMC6352 omni_wheel
Fork of KANI2017v2 by
bot/PIDcontroller/PID_controller.cpp@19:41f7dd1a5ed1, 2017-08-31 (annotated)
- Committer:
- uchitake
- Date:
- Thu Aug 31 10:34:55 2017 +0900
- Revision:
- 19:41f7dd1a5ed1
- Parent:
- 6:590c9622ecf1
- Child:
- 20:477c5d039e93
fix PIDcontroller class
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 | 6:590c9622ecf1 | 8 | PIDC::PIDC() : PID(KC, TI, TD, INTERVAL), HMC6352(HMCsda, HMCscl) |
UCHITAKE | 6:590c9622ecf1 | 9 | { |
uchitake |
19:41f7dd1a5ed1 | 10 | PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT); |
uchitake |
19:41f7dd1a5ed1 | 11 | PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT); |
uchitake |
19:41f7dd1a5ed1 | 12 | PID::setBias(BIAS); |
uchitake |
19:41f7dd1a5ed1 | 13 | PID::setMode(AUTO_MODE); |
uchitake |
19:41f7dd1a5ed1 | 14 | PID::setSetPoint(0.0); |
UCHITAKE | 6:590c9622ecf1 | 15 | |
uchitake |
19:41f7dd1a5ed1 | 16 | HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20); |
uchitake |
19:41f7dd1a5ed1 | 17 | rawDegree = HMC6352::sample(); |
uchitake |
19:41f7dd1a5ed1 | 18 | beforeDegree = HMC6352::sample(); |
uchitake |
19:41f7dd1a5ed1 | 19 | offSetDegree = HMC6352::sample(); |
UCHITAKE | 6:590c9622ecf1 | 20 | initDegree = 0; |
UCHITAKE | 6:590c9622ecf1 | 21 | turnOverNumber = 0; |
UCHITAKE | 6:590c9622ecf1 | 22 | // this -> attach(this, &PIDC::updateOutput, INTERVAL); |
UCHITAKE | 6:590c9622ecf1 | 23 | } |
UCHITAKE | 6:590c9622ecf1 | 24 | |
uchitake |
19:41f7dd1a5ed1 | 25 | PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) : |
uchitake |
19:41f7dd1a5ed1 | 26 | PID(kc, ti, td, interval), HMC6352(sda, scl) |
uchitake |
19:41f7dd1a5ed1 | 27 | { |
uchitake |
19:41f7dd1a5ed1 | 28 | PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT); |
uchitake |
19:41f7dd1a5ed1 | 29 | PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT); |
uchitake |
19:41f7dd1a5ed1 | 30 | PID::setBias(BIAS); |
uchitake |
19:41f7dd1a5ed1 | 31 | PID::setMode(AUTO_MODE); |
uchitake |
19:41f7dd1a5ed1 | 32 | PID::setSetPoint(0.0); |
uchitake |
19:41f7dd1a5ed1 | 33 | |
uchitake |
19:41f7dd1a5ed1 | 34 | HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20); |
uchitake |
19:41f7dd1a5ed1 | 35 | rawDegree = HMC6352::sample(); |
uchitake |
19:41f7dd1a5ed1 | 36 | beforeDegree = HMC6352::sample(); |
uchitake |
19:41f7dd1a5ed1 | 37 | offSetDegree = HMC6352::sample(); |
uchitake |
19:41f7dd1a5ed1 | 38 | initDegree = 0; |
uchitake |
19:41f7dd1a5ed1 | 39 | turnOverNumber = 0; |
uchitake |
19:41f7dd1a5ed1 | 40 | // this -> attach(this, &PIDC::updateOutput, INTERVAL); |
uchitake |
19:41f7dd1a5ed1 | 41 | } |
uchitake |
19:41f7dd1a5ed1 | 42 | |
uchitake |
19:41f7dd1a5ed1 | 43 | |
UCHITAKE | 6:590c9622ecf1 | 44 | void PIDC::confirm() |
UCHITAKE | 6:590c9622ecf1 | 45 | { |
uchitake |
19:41f7dd1a5ed1 | 46 | rawDegree = HMC6352::sample(); |
UCHITAKE | 6:590c9622ecf1 | 47 | if(rawDegree - beforeDegree < -1800) turnOverNumber++; |
UCHITAKE | 6:590c9622ecf1 | 48 | if(rawDegree - beforeDegree > 1800) turnOverNumber--; |
UCHITAKE | 6:590c9622ecf1 | 49 | initDegree = rawDegree - offSetDegree + turnOverNumber * 3600; |
uchitake |
19:41f7dd1a5ed1 | 50 | beforeDegree = HMC6352::sample(); |
uchitake |
19:41f7dd1a5ed1 | 51 | PID::setProcessValue(initDegree / 10.0); |
uchitake |
19:41f7dd1a5ed1 | 52 | co = PID::compute(); |
UCHITAKE | 6:590c9622ecf1 | 53 | } |
UCHITAKE | 6:590c9622ecf1 | 54 | |
UCHITAKE | 6:590c9622ecf1 | 55 | float PIDC::getCo() |
UCHITAKE | 6:590c9622ecf1 | 56 | { |
UCHITAKE | 6:590c9622ecf1 | 57 | return co; |
uchitake |
19:41f7dd1a5ed1 | 58 | } |