NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   2017NHKpin_config FEP HMC6352 PID QEI R1307 ikarashiMDC omni_wheel

Fork of KANIv3 by NagaokaRoboticsClub_mbedTeam

bot/PIDcontroller/PID_controller.cpp@20:aaadf3cfcad1, 2017-09-28 (annotated)

Committer:
uchitake
Date:
Thu Sep 28 20:59:22 2017 +0900
Revision:
20:aaadf3cfcad1
Parent:
17:79fa65706f92
Child:
23:797d25f3df5e
Child:
32:b619c7787dc3
merge develop branch

Who changed what in which revision?

UserRevisionLine numberNew contents of line
uchitake 1:845af5425eec 1 #include "PID_controller.h"
uchitake 1:845af5425eec 2
uchitake 1:845af5425eec 3 void PIDC::updateOutput()
uchitake 1:845af5425eec 4 {
uchitake 1:845af5425eec 5 confirm();
uchitake 1:845af5425eec 6 }
uchitake 1:845af5425eec 7
uchitake 1:845af5425eec 8 PIDC::PIDC() :
uchitake 1:845af5425eec 9 PID(KC, TI, TD, INTERVAL),
uchitake 1:845af5425eec 10 HMC6352(HMCsda, HMCscl),
uchitake 6:fe9767a50891 11 axisOffSetDegree(0),
uchitake 6:fe9767a50891 12 planeOffSetDegree(0),
uchitake 1:845af5425eec 13 turnOverNumber(0),
uchitake 1:845af5425eec 14 beforeDegree(0),
uchitake 6:fe9767a50891 15 rawDegree(0),
uchitake 6:fe9767a50891 16 calculationResult(0),
uchitake 6:fe9767a50891 17 axisCurrentDegree(0),
uchitake 6:fe9767a50891 18 planeCurrentDegree(0)
uchitake 1:845af5425eec 19 {
uchitake 1:845af5425eec 20 PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
uchitake 1:845af5425eec 21 PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
uchitake 1:845af5425eec 22 PID::setBias(BIAS);
uchitake 1:845af5425eec 23 PID::setMode(AUTO_MODE);
uchitake 1:845af5425eec 24 PID::setSetPoint(0.0);
uchitake 1:845af5425eec 25
uchitake 5:16ea97725085 26 wait(0.1);
uchitake 1:845af5425eec 27 HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
uchitake 5:16ea97725085 28 wait(0.1);
uchitake 1:845af5425eec 29 rawDegree = HMC6352::sample();
uchitake 5:16ea97725085 30 beforeDegree = rawDegree;
uchitake 6:fe9767a50891 31 planeOffSetDegree = rawDegree;
uchitake 6:fe9767a50891 32 axisOffSetDegree = rawDegree;
uchitake 1:845af5425eec 33 // this -> attach(this, &PIDC::updateOutput, INTERVAL);
uchitake 1:845af5425eec 34 }
uchitake 1:845af5425eec 35
uchitake 1:845af5425eec 36 PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) :
uchitake 1:845af5425eec 37 PID(kc, ti, td, interval),
uchitake 1:845af5425eec 38 HMC6352(sda, scl),
uchitake 6:fe9767a50891 39 axisOffSetDegree(0),
uchitake 6:fe9767a50891 40 planeOffSetDegree(0),
uchitake 1:845af5425eec 41 turnOverNumber(0),
uchitake 1:845af5425eec 42 beforeDegree(0),
uchitake 6:fe9767a50891 43 rawDegree(0),
uchitake 6:fe9767a50891 44 calculationResult(0),
uchitake 6:fe9767a50891 45 axisCurrentDegree(0),
uchitake 6:fe9767a50891 46 planeCurrentDegree(0)
uchitake 1:845af5425eec 47 {
uchitake 1:845af5425eec 48 PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
uchitake 1:845af5425eec 49 PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
uchitake 1:845af5425eec 50 PID::setBias(BIAS);
uchitake 1:845af5425eec 51 PID::setMode(AUTO_MODE);
uchitake 1:845af5425eec 52 PID::setSetPoint(0.0);
uchitake 1:845af5425eec 53
uchitake 5:16ea97725085 54 wait(0.1);
uchitake 1:845af5425eec 55 HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
uchitake 5:16ea97725085 56 wait(0.1);
uchitake 1:845af5425eec 57 rawDegree = HMC6352::sample();
uchitake 5:16ea97725085 58 beforeDegree = rawDegree;
uchitake 6:fe9767a50891 59 planeOffSetDegree = rawDegree;
uchitake 6:fe9767a50891 60 axisOffSetDegree = rawDegree;
uchitake 1:845af5425eec 61 // this -> attach(this, &PIDC::updateOutput, INTERVAL);
uchitake 1:845af5425eec 62 }
uchitake 1:845af5425eec 63
uchitake 1:845af5425eec 64
uchitake 1:845af5425eec 65 void PIDC::confirm()
uchitake 1:845af5425eec 66 {
uchitake 1:845af5425eec 67 rawDegree = HMC6352::sample();
uchitake 6:fe9767a50891 68 if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber;
uchitake 6:fe9767a50891 69 if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber;
uchitake 6:fe9767a50891 70 axisCurrentDegree = rawDegree - axisOffSetDegree + turnOverNumber * 3600;
uchitake 6:fe9767a50891 71 planeCurrentDegree = rawDegree - planeOffSetDegree + turnOverNumber * 3600;
uchitake 1:845af5425eec 72 beforeDegree = HMC6352::sample();
uchitake 6:fe9767a50891 73 PID::setProcessValue(axisCurrentDegree / 10.0);
uchitake 6:fe9767a50891 74 calculationResult = PID::compute();
uchitake 1:845af5425eec 75 }
uchitake 1:845af5425eec 76
uchitake 6:fe9767a50891 77 float PIDC::getCalculationResult() const
uchitake 1:845af5425eec 78 {
uchitake 6:fe9767a50891 79 return calculationResult;
uchitake 1:845af5425eec 80 }
uchitake 1:845af5425eec 81
uchitake 6:fe9767a50891 82 int PIDC::getCurrentDegree() const
uchitake 3:369d9ee17e84 83 {
uchitake 6:fe9767a50891 84 return planeCurrentDegree;
uchitake 3:369d9ee17e84 85 }
uchitake 3:369d9ee17e84 86
uchitake 17:79fa65706f92 87 int PIDC::getRawDegree()
uchitake 17:79fa65706f92 88 {
uchitake 17:79fa65706f92 89 return HMC6352::sample();
uchitake 17:79fa65706f92 90 }
uchitake 17:79fa65706f92 91
uchitake 6:fe9767a50891 92 void PIDC::resetAxisOffset()
uchitake 5:16ea97725085 93 {
uchitake 5:16ea97725085 94 beforeDegree = HMC6352::sample();
uchitake 6:fe9767a50891 95 axisOffSetDegree = HMC6352::sample();
uchitake 5:16ea97725085 96 turnOverNumber = 0;
uchitake 5:16ea97725085 97 }
uchitake 5:16ea97725085 98
uchitake 6:fe9767a50891 99 void PIDC::resetPlaneOffset()
uchitake 5:16ea97725085 100 {
uchitake 5:16ea97725085 101 beforeDegree = HMC6352::sample();
uchitake 6:fe9767a50891 102 planeOffSetDegree = HMC6352::sample();
uchitake 5:16ea97725085 103 turnOverNumber = 0;
uchitake 5:16ea97725085 104 }
uchitake 1:845af5425eec 105
uchitake 1:845af5425eec 106 void PIDC::calibration(int mode)
uchitake 1:845af5425eec 107 {
uchitake 1:845af5425eec 108 setCalibrationMode(mode);
uchitake 1:845af5425eec 109 }