NHK2017 octopus robot
Dependencies: 2017NHKpin_config mbed FEP ikarashiMDC PID jy901 omni HMC6352 omni_wheel
Fork of KANI2017v2 by
bot/PIDcontroller/PID_controller.cpp@20:477c5d039e93, 2017-09-05 (annotated)
- Committer:
- number_key
- Date:
- Tue Sep 05 01:23:32 2017 +0000
- Revision:
- 20:477c5d039e93
- Parent:
- 19:41f7dd1a5ed1
- Child:
- 22:682cc376da6f
start
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 | 20:477c5d039e93 | 8 | PIDC::PIDC() : PID(KC, TI, TD, INTERVAL), JY901(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 | |
number_key | 20:477c5d039e93 | 16 | //HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20); |
number_key | 20:477c5d039e93 | 17 | rawDegree = JY901::getZaxisAngle(); |
number_key | 20:477c5d039e93 | 18 | beforeDegree = JY901::getZaxisAngle(); |
number_key | 20:477c5d039e93 | 19 | offSetDegree = JY901::getZaxisAngle(); |
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) : |
number_key | 20:477c5d039e93 | 26 | PID(kc, ti, td, interval), JY901(sda, scl) |
number_key | 20:477c5d039e93 | 27 | { |
number_key | 20:477c5d039e93 | 28 | PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT); |
number_key | 20:477c5d039e93 | 29 | PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT); |
number_key | 20:477c5d039e93 | 30 | PID::setBias(BIAS); |
number_key | 20:477c5d039e93 | 31 | PID::setMode(AUTO_MODE); |
number_key | 20:477c5d039e93 | 32 | PID::setSetPoint(0.0); |
number_key | 20:477c5d039e93 | 33 | |
number_key | 20:477c5d039e93 | 34 | //HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20); |
number_key | 20:477c5d039e93 | 35 | rawDegree = JY901::getZaxisAngle(); |
number_key | 20:477c5d039e93 | 36 | beforeDegree = JY901::getZaxisAngle(); |
number_key | 20:477c5d039e93 | 37 | offSetDegree = JY901::getZaxisAngle(); |
number_key | 20:477c5d039e93 | 38 | initDegree = 0; |
number_key | 20:477c5d039e93 | 39 | turnOverNumber = 0; |
number_key | 20:477c5d039e93 | 40 | // this -> attach(this, &PIDC::updateOutput, INTERVAL); |
number_key | 20:477c5d039e93 | 41 | } |
uchitake |
19:41f7dd1a5ed1 | 42 | |
uchitake |
19:41f7dd1a5ed1 | 43 | |
UCHITAKE | 6:590c9622ecf1 | 44 | void PIDC::confirm() |
UCHITAKE | 6:590c9622ecf1 | 45 | { |
number_key | 20:477c5d039e93 | 46 | rawDegree = getZaxisAngle(); |
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; |
number_key | 20:477c5d039e93 | 50 | beforeDegree = JY901::getZaxisAngle(); |
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 | } |