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@23:37bb9afe9fdc, 2017-09-13 (annotated)

Committer:
number_key
Date:
Wed Sep 13 14:26:47 2017 +0900
Revision:
23:37bb9afe9fdc
Parent:
22:682cc376da6f
Child:
26:7258d5ad0bff
move

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