NagaokaRoboticsClub_mbedTeam / Mbed OS KANIv3

kani

Dependencies:   2017NHKpin_config FEP omni_wheel PID R1307 ikarashiMDC

classDiagram

　   ＼￣＼　　　　　　　　　　　　　    　 ／￣／ 
/l　    ＼　　＼　　 　　　　　　　　　 ／　　／　lヽ　 
|　ヽ 　ヽ　　 |　　　　 　　　　　　｜　　/　　/　| 
＼　` ‐ヽ　　ヽ　　●　　      ●    　　   /　　/　‐　 ／ 
　 ＼ __　l　　|　　｜|＿＿＿|｜ /　　l　__ ／ 
　　　 　＼　 ＼ ／　　　　　　＼／ 
　　　　　 ／＼|　　 人＿＿人　　|／＼　　　　<ｽﾞﾜｲｶﾞﾆ　 
　　　　／／＼|　　　　　      　　|／＼＼　　　　 
　　　　／／＼|　　　ｹｶﾞﾆ           　|／＼＼　　　　 
　　　　／　  　 . ＼＿＿＿＿＿／　 　      ＼ 

　 　 　 　 　 　 　 　 　 　 　 　    　 　 ┏┓　 　 　 　 ┏━┓┏┓　 　 　 　 　 　 　 
     ┏┓　      　 ┏┓┏┓   ┏┓　 　 ┏┓┗┛   　 ┏┓　┗┓┃┗┛　 　 　 　 　 　 　 
┏┛┗━┓ 　┃┃┃┃    ┃┃┏━┛┗┓┏┓┏┛┗━┓┃┃┏┓┏┓┏━━━┓ 
┗┓┏━┛　 ┃┃┗┛    ┃┃┗━┓┏┛┗┛┗┓┏┓┃┗┛┗┛┃┃┗━━━┛　 　 
┏┛┃┏━┓┃┗━━┓┃┃┏━┛┗┓ 　    ┏┛┃┃┃  　 　 　 ┃┃　 　 　 　 　 　 　 
┃┏┛┗━┛┗━━┓┃┃┃┃┏┓┏┛ 　    ┗━┛┃┃  　 　 　 ┃┃┏┓　 　 　 　 　 
┃┃┏━━┓┏━━┛┃┃┃┃┗┛┃　 　  　 　 ┏┛┃  　 　 　 ┃┃┃┗━━┓　 　 
┗┛┗━━┛┗━━━┛┗┛┗━━┛　 　  　 　 ┗━┛　   　 　 ┗┛┗━━━┛　

bot/PIDcontroller/PID_controller.cpp@23:797d25f3df5e, 2017-10-01 (annotated)

Committer:
takeuchi
Date:
Sun Oct 01 11:43:53 2017 +0900
Branch:
develop1
Revision:
23:797d25f3df5e
Parent:
17:79fa65706f92 
fix inheritance problem

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 PIDC::PIDC() :
takeuchi 23:797d25f3df5e 4 offSetDegree(0),
uchitake 1:845af5425eec 5 turnOverNumber(0),
uchitake 1:845af5425eec 6 beforeDegree(0),
uchitake 6:fe9767a50891 7 rawDegree(0),
uchitake 6:fe9767a50891 8 calculationResult(0),
takeuchi 23:797d25f3df5e 9 currentDegree(0)
uchitake 1:845af5425eec 10 {
takeuchi 23:797d25f3df5e 11 pid = new PID(KC, TI, TD, INTERVAL);
takeuchi 23:797d25f3df5e 12 pid -> setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
takeuchi 23:797d25f3df5e 13 pid -> setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
takeuchi 23:797d25f3df5e 14 pid -> setBias(BIAS);
takeuchi 23:797d25f3df5e 15 pid -> setMode(AUTO_MODE);
takeuchi 23:797d25f3df5e 16 pid -> setSetPoint(0.0);
uchitake 1:845af5425eec 17
uchitake 5:16ea97725085 18 wait(0.1);
takeuchi 23:797d25f3df5e 19 hmc -> setOpMode(HMC6352_CONTINUOUS, 1, 20);
uchitake 5:16ea97725085 20 wait(0.1);
takeuchi 23:797d25f3df5e 21 rawDegree = hmc -> sample();
uchitake 5:16ea97725085 22 beforeDegree = rawDegree;
takeuchi 23:797d25f3df5e 23 offSetDegree = rawDegree;
uchitake 1:845af5425eec 24 }
uchitake 1:845af5425eec 25
uchitake 1:845af5425eec 26 PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) :
takeuchi 23:797d25f3df5e 27 offSetDegree(0),
uchitake 1:845af5425eec 28 turnOverNumber(0),
uchitake 1:845af5425eec 29 beforeDegree(0),
uchitake 6:fe9767a50891 30 rawDegree(0),
uchitake 6:fe9767a50891 31 calculationResult(0),
takeuchi 23:797d25f3df5e 32 currentDegree(0)
uchitake 1:845af5425eec 33 {
takeuchi 23:797d25f3df5e 34 pid = new PID(kc, ti, td, interval);
takeuchi 23:797d25f3df5e 35 hmc = new HMC6352(sda, scl);
takeuchi 23:797d25f3df5e 36 pid -> setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
takeuchi 23:797d25f3df5e 37 pid -> setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
takeuchi 23:797d25f3df5e 38 pid -> setBias(BIAS);
takeuchi 23:797d25f3df5e 39 pid -> setMode(AUTO_MODE);
takeuchi 23:797d25f3df5e 40 pid -> setSetPoint(0.0);
uchitake 1:845af5425eec 41
uchitake 5:16ea97725085 42 wait(0.1);
takeuchi 23:797d25f3df5e 43 hmc -> setOpMode(HMC6352_CONTINUOUS, 1, 20);
uchitake 5:16ea97725085 44 wait(0.1);
takeuchi 23:797d25f3df5e 45 rawDegree = hmc -> sample();
uchitake 5:16ea97725085 46 beforeDegree = rawDegree;
takeuchi 23:797d25f3df5e 47 offSetDegree = rawDegree;
uchitake 1:845af5425eec 48 }
uchitake 1:845af5425eec 49
uchitake 1:845af5425eec 50
uchitake 1:845af5425eec 51 void PIDC::confirm()
uchitake 1:845af5425eec 52 {
takeuchi 23:797d25f3df5e 53 rawDegree = hmc -> sample();
uchitake 6:fe9767a50891 54 if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber;
uchitake 6:fe9767a50891 55 if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber;
takeuchi 23:797d25f3df5e 56 currentDegree = rawDegree - offSetDegree + turnOverNumber * 3600;
takeuchi 23:797d25f3df5e 57 beforeDegree = hmc -> sample();
takeuchi 23:797d25f3df5e 58 pid -> setProcessValue(currentDegree / 10.0);
takeuchi 23:797d25f3df5e 59 calculationResult = pid -> compute();
takeuchi 23:797d25f3df5e 60 }
takeuchi 23:797d25f3df5e 61
takeuchi 23:797d25f3df5e 62 void PIDC::setPoint(float point)
takeuchi 23:797d25f3df5e 63 {
takeuchi 23:797d25f3df5e 64 pid -> setSetPoint(point);
uchitake 1:845af5425eec 65 }
uchitake 1:845af5425eec 66
uchitake 6:fe9767a50891 67 float PIDC::getCalculationResult() const
uchitake 1:845af5425eec 68 {
uchitake 6:fe9767a50891 69 return calculationResult;
uchitake 1:845af5425eec 70 }
uchitake 1:845af5425eec 71
uchitake 6:fe9767a50891 72 int PIDC::getCurrentDegree() const
uchitake 3:369d9ee17e84 73 {
takeuchi 23:797d25f3df5e 74 return currentDegree;
uchitake 3:369d9ee17e84 75 }
uchitake 3:369d9ee17e84 76
uchitake 17:79fa65706f92 77 int PIDC::getRawDegree()
uchitake 17:79fa65706f92 78 {
takeuchi 23:797d25f3df5e 79 return hmc -> sample();
uchitake 17:79fa65706f92 80 }
uchitake 17:79fa65706f92 81
takeuchi 23:797d25f3df5e 82 void PIDC::resetOffset()
uchitake 5:16ea97725085 83 {
takeuchi 23:797d25f3df5e 84 beforeDegree = hmc -> sample();
takeuchi 23:797d25f3df5e 85 offSetDegree = hmc -> sample();
uchitake 5:16ea97725085 86 turnOverNumber = 0;
uchitake 5:16ea97725085 87 }
uchitake 1:845af5425eec 88
uchitake 1:845af5425eec 89 void PIDC::calibration(int mode)
uchitake 1:845af5425eec 90 {
takeuchi 23:797d25f3df5e 91 hmc -> setCalibrationMode(mode);
uchitake 1:845af5425eec 92 }