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@20:477c5d039e93, 2017-09-05 (annotated)

Committer:
number_key
Date:
Tue Sep 05 01:23:32 2017 +0000
Revision:
20:477c5d039e93
Parent:
19:41f7dd1a5ed1
Child:
22:682cc376da6f
start

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