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@19:41f7dd1a5ed1, 2017-08-31 (annotated)

Committer:
uchitake
Date:
Thu Aug 31 10:34:55 2017 +0900
Revision:
19:41f7dd1a5ed1
Parent:
6:590c9622ecf1
Child:
20:477c5d039e93
fix PIDcontroller class

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
UCHITAKE 6:590c9622ecf1 8 PIDC::PIDC() : PID(KC, TI, TD, INTERVAL), HMC6352(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
uchitake 19:41f7dd1a5ed1 16 HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
uchitake 19:41f7dd1a5ed1 17 rawDegree = HMC6352::sample();
uchitake 19:41f7dd1a5ed1 18 beforeDegree = HMC6352::sample();
uchitake 19: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 19:41f7dd1a5ed1 25 PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) :
uchitake 19:41f7dd1a5ed1 26 PID(kc, ti, td, interval), HMC6352(sda, scl)
uchitake 19:41f7dd1a5ed1 27 {
uchitake 19:41f7dd1a5ed1 28 PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
uchitake 19:41f7dd1a5ed1 29 PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
uchitake 19:41f7dd1a5ed1 30 PID::setBias(BIAS);
uchitake 19:41f7dd1a5ed1 31 PID::setMode(AUTO_MODE);
uchitake 19:41f7dd1a5ed1 32 PID::setSetPoint(0.0);
uchitake 19:41f7dd1a5ed1 33
uchitake 19:41f7dd1a5ed1 34 HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
uchitake 19:41f7dd1a5ed1 35 rawDegree = HMC6352::sample();
uchitake 19:41f7dd1a5ed1 36 beforeDegree = HMC6352::sample();
uchitake 19:41f7dd1a5ed1 37 offSetDegree = HMC6352::sample();
uchitake 19:41f7dd1a5ed1 38 initDegree = 0;
uchitake 19:41f7dd1a5ed1 39 turnOverNumber = 0;
uchitake 19:41f7dd1a5ed1 40 // this -> attach(this, &PIDC::updateOutput, INTERVAL);
uchitake 19:41f7dd1a5ed1 41 }
uchitake 19:41f7dd1a5ed1 42
uchitake 19:41f7dd1a5ed1 43
UCHITAKE 6:590c9622ecf1 44 void PIDC::confirm()
UCHITAKE 6:590c9622ecf1 45 {
uchitake 19: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 19:41f7dd1a5ed1 50 beforeDegree = HMC6352::sample();
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 }