NHK2017Ateamかにろぼ
Dependencies: 2017NHKpin_config mbed FEP HMC6352 MotorDriverController PID QEI omni
\ ̄\ / ̄/ /l \ \ / / lヽ | ヽ ヽ | | / / | \ ` ‐ヽ ヽ ● ● / / ‐ / \ __ l | ||___|| / l __ / \ \ / \/ /\| 人__人 |/\ //\| |/\\ //\| |/\\ / . \_____/ \ ┏┓ ┏━┓┏┓ ┏┓ ┏┓┏┓ ┏┓ ┏┓┗┛ ┏┓ ┗┓┃┗┛ ┏┛┗━┓ ┃┃┃┃ ┃┃┏━┛┗┓┏┓┏┛┗━┓┃┃┏┓┏┓┏━━━┓ ┗┓┏━┛ ┃┃┗┛ ┃┃┗━┓┏┛┗┛┗┓┏┓┃┗┛┗┛┃┃┗━━━┛ ┏┛┃┏━┓┃┗━━┓┃┃┏━┛┗┓ ┏┛┃┃┃ ┃┃ ┃┏┛┗━┛┗━━┓┃┃┃┃┏┓┏┛ ┗━┛┃┃ ┃┃┏┓ ┃┃┏━━┓┏━━┛┃┃┃┃┗┛┃ ┏┛┃ ┃┃┃┗━━┓ ┗┛┗━━┛┗━━━┛┗┛┗━━┛ ┗━┛ ┗┛┗━━━┛
bot/PIDcontroller/PID_controller.cpp@19:3a62cbc6fee9, 2017-09-04 (annotated)
- Committer:
- UCHITAKE
- Date:
- Mon Sep 04 01:01:13 2017 +0000
- Revision:
- 19:3a62cbc6fee9
- Parent:
- 18:41f7dd1a5ed1
- Child:
- 22:bb6afe7332c3
add calibration
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 |
18:41f7dd1a5ed1 | 10 | PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT); |
uchitake |
18:41f7dd1a5ed1 | 11 | PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT); |
uchitake |
18:41f7dd1a5ed1 | 12 | PID::setBias(BIAS); |
uchitake |
18:41f7dd1a5ed1 | 13 | PID::setMode(AUTO_MODE); |
uchitake |
18:41f7dd1a5ed1 | 14 | PID::setSetPoint(0.0); |
UCHITAKE | 6:590c9622ecf1 | 15 | |
uchitake |
18:41f7dd1a5ed1 | 16 | HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20); |
uchitake |
18:41f7dd1a5ed1 | 17 | rawDegree = HMC6352::sample(); |
uchitake |
18:41f7dd1a5ed1 | 18 | beforeDegree = HMC6352::sample(); |
uchitake |
18: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 |
18:41f7dd1a5ed1 | 25 | PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) : |
UCHITAKE | 19:3a62cbc6fee9 | 26 | PID(kc, ti, td, interval), HMC6352(sda, scl) |
UCHITAKE | 19:3a62cbc6fee9 | 27 | { |
UCHITAKE | 19:3a62cbc6fee9 | 28 | PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT); |
UCHITAKE | 19:3a62cbc6fee9 | 29 | PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT); |
UCHITAKE | 19:3a62cbc6fee9 | 30 | PID::setBias(BIAS); |
UCHITAKE | 19:3a62cbc6fee9 | 31 | PID::setMode(AUTO_MODE); |
UCHITAKE | 19:3a62cbc6fee9 | 32 | PID::setSetPoint(0.0); |
UCHITAKE | 19:3a62cbc6fee9 | 33 | |
UCHITAKE | 19:3a62cbc6fee9 | 34 | HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20); |
UCHITAKE | 19:3a62cbc6fee9 | 35 | rawDegree = HMC6352::sample(); |
UCHITAKE | 19:3a62cbc6fee9 | 36 | beforeDegree = HMC6352::sample(); |
UCHITAKE | 19:3a62cbc6fee9 | 37 | offSetDegree = HMC6352::sample(); |
UCHITAKE | 19:3a62cbc6fee9 | 38 | initDegree = 0; |
UCHITAKE | 19:3a62cbc6fee9 | 39 | turnOverNumber = 0; |
UCHITAKE | 19:3a62cbc6fee9 | 40 | // this -> attach(this, &PIDC::updateOutput, INTERVAL); |
UCHITAKE | 19:3a62cbc6fee9 | 41 | } |
uchitake |
18:41f7dd1a5ed1 | 42 | |
uchitake |
18:41f7dd1a5ed1 | 43 | |
UCHITAKE | 6:590c9622ecf1 | 44 | void PIDC::confirm() |
UCHITAKE | 6:590c9622ecf1 | 45 | { |
uchitake |
18: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 |
18:41f7dd1a5ed1 | 50 | beforeDegree = HMC6352::sample(); |
uchitake |
18:41f7dd1a5ed1 | 51 | PID::setProcessValue(initDegree / 10.0); |
uchitake |
18: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 |
18:41f7dd1a5ed1 | 58 | } |
UCHITAKE | 19:3a62cbc6fee9 | 59 | |
UCHITAKE | 19:3a62cbc6fee9 | 60 | |
UCHITAKE | 19:3a62cbc6fee9 | 61 | void PIDC::calibration(int mode) |
UCHITAKE | 19:3a62cbc6fee9 | 62 | { |
UCHITAKE | 19:3a62cbc6fee9 | 63 | setCalibrationMode(mode); |
UCHITAKE | 19:3a62cbc6fee9 | 64 | } |