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@22:682cc376da6f, 2017-09-07 (annotated)

Committer:
number_key
Date:
Thu Sep 07 06:26:53 2017 +0000
Revision:
22:682cc376da6f
Parent:
20:477c5d039e93
Child:
23:37bb9afe9fdc
save

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