NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated NHK2017_octopus

NHK2017 octopus robot

Dependencies:   2017NHKpin_config mbed FEP ikarashiMDC PID jy901 omni HMC6352 omni_wheel

Fork of KANI2017v2 by NagaokaRoboticsClub_mbedTeam

bot/PIDcontroller/PID_controller.cpp@26:7258d5ad0bff, 2017-10-25 (annotated)

Committer:
number_key
Date:
Wed Oct 25 03:25:06 2017 +0900
Revision:
26:7258d5ad0bff
Parent:
23:37bb9afe9fdc 
no message

Who changed what in which revision?

UserRevisionLine numberNew 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 26:7258d5ad0bff 10 HMC6352(HMCsda, HMCscl),
number_key 26:7258d5ad0bff 11 offSetDegree(0),
number_key 22:682cc376da6f 12 turnOverNumber(0),
number_key 22:682cc376da6f 13 beforeDegree(0),
number_key 22:682cc376da6f 14 rawDegree(0),
number_key 22:682cc376da6f 15 calculationResult(0),
number_key 26:7258d5ad0bff 16 currentDegree(0)
UCHITAKE 6:590c9622ecf1 17 {
uchitake 19:41f7dd1a5ed1 18 PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
uchitake 19:41f7dd1a5ed1 19 PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
uchitake 19:41f7dd1a5ed1 20 PID::setBias(BIAS);
uchitake 19:41f7dd1a5ed1 21 PID::setMode(AUTO_MODE);
uchitake 19:41f7dd1a5ed1 22 PID::setSetPoint(0.0);
UCHITAKE 6:590c9622ecf1 23
number_key 26:7258d5ad0bff 24 wait(0.1);
number_key 26:7258d5ad0bff 25 HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
number_key 26:7258d5ad0bff 26 wait(0.1);
number_key 26:7258d5ad0bff 27 rawDegree = HMC6352::sample();
number_key 22:682cc376da6f 28 beforeDegree = rawDegree;
number_key 26:7258d5ad0bff 29 offSetDegree = rawDegree;
UCHITAKE 6:590c9622ecf1 30 }
UCHITAKE 6:590c9622ecf1 31
uchitake 19:41f7dd1a5ed1 32 PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) :
number_key 22:682cc376da6f 33 PID(kc, ti, td, interval),
number_key 26:7258d5ad0bff 34 HMC6352(sda, scl),
number_key 26:7258d5ad0bff 35 offSetDegree(0),
number_key 22:682cc376da6f 36 turnOverNumber(0),
number_key 22:682cc376da6f 37 beforeDegree(0),
number_key 22:682cc376da6f 38 rawDegree(0),
number_key 22:682cc376da6f 39 calculationResult(0),
number_key 26:7258d5ad0bff 40 currentDegree(0)
number_key 20:477c5d039e93 41 {
number_key 20:477c5d039e93 42 PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
number_key 20:477c5d039e93 43 PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
number_key 20:477c5d039e93 44 PID::setBias(BIAS);
number_key 20:477c5d039e93 45 PID::setMode(AUTO_MODE);
number_key 20:477c5d039e93 46 PID::setSetPoint(0.0);
number_key 20:477c5d039e93 47
number_key 26:7258d5ad0bff 48 wait(0.1);
number_key 26:7258d5ad0bff 49 HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
number_key 26:7258d5ad0bff 50 wait(0.1);
number_key 26:7258d5ad0bff 51 rawDegree = HMC6352::sample();
number_key 22:682cc376da6f 52 beforeDegree = rawDegree;
number_key 26:7258d5ad0bff 53 offSetDegree = rawDegree;
number_key 20:477c5d039e93 54 }
uchitake 19:41f7dd1a5ed1 55
uchitake 19:41f7dd1a5ed1 56
UCHITAKE 6:590c9622ecf1 57 void PIDC::confirm()
UCHITAKE 6:590c9622ecf1 58 {
number_key 26:7258d5ad0bff 59 rawDegree = HMC6352::sample();
number_key 22:682cc376da6f 60 if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber;
number_key 22:682cc376da6f 61 if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber;
number_key 26:7258d5ad0bff 62 currentDegree = rawDegree - offSetDegree + turnOverNumber * 3600;
number_key 26:7258d5ad0bff 63 beforeDegree = HMC6352::sample();
number_key 26:7258d5ad0bff 64 PID::setProcessValue(currentDegree / 10.0);
number_key 22:682cc376da6f 65 calculationResult = PID::compute();
number_key 22:682cc376da6f 66 }
number_key 22:682cc376da6f 67
number_key 22:682cc376da6f 68 float PIDC::getCalculationResult() const
number_key 22:682cc376da6f 69 {
number_key 22:682cc376da6f 70 return calculationResult;
UCHITAKE 6:590c9622ecf1 71 }
UCHITAKE 6:590c9622ecf1 72
number_key 26:7258d5ad0bff 73 int PIDC::getCurrentDegree() const
number_key 22:682cc376da6f 74 {
number_key 26:7258d5ad0bff 75 return currentDegree;
number_key 26:7258d5ad0bff 76 }
number_key 26:7258d5ad0bff 77
number_key 26:7258d5ad0bff 78 int PIDC::getRawDegree()
number_key 26:7258d5ad0bff 79 {
number_key 26:7258d5ad0bff 80 return HMC6352::sample();
number_key 22:682cc376da6f 81 }
number_key 22:682cc376da6f 82
number_key 26:7258d5ad0bff 83 void PIDC::setPoint(float point)
UCHITAKE 6:590c9622ecf1 84 {
number_key 26:7258d5ad0bff 85 PID::setSetPoint(point);
uchitake 19:41f7dd1a5ed1 86 }
number_key 22:682cc376da6f 87
number_key 26:7258d5ad0bff 88 void PIDC::resetOffset()
number_key 22:682cc376da6f 89 {
number_key 26:7258d5ad0bff 90 beforeDegree = HMC6352::sample();
number_key 26:7258d5ad0bff 91 offSetDegree = HMC6352::sample();
number_key 22:682cc376da6f 92 turnOverNumber = 0;
number_key 22:682cc376da6f 93 }
number_key 22:682cc376da6f 94
number_key 22:682cc376da6f 95 void PIDC::calibration(int mode)
number_key 22:682cc376da6f 96 {
number_key 26:7258d5ad0bff 97 setCalibrationMode(mode);
number_key 22:682cc376da6f 98 }