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@5:16ea97725085, 2017-09-06 (annotated)

Committer:
uchitake
Date:
Wed Sep 06 17:47:20 2017 +0900
Revision:
5:16ea97725085
Parent:
3:369d9ee17e84
Child:
6:fe9767a50891
working  MEGA  STUPID CODE

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 1:845af5425eec 11 rawDegree(0),
uchitake 1:845af5425eec 12 offSetDegree(0),
uchitake 1:845af5425eec 13 turnOverNumber(0),
uchitake 1:845af5425eec 14 beforeDegree(0),
uchitake 1:845af5425eec 15 co(0),
uchitake 1:845af5425eec 16 processValue(0),
uchitake 1:845af5425eec 17 initDegree(0)
uchitake 1:845af5425eec 18 {
uchitake 1:845af5425eec 19 PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
uchitake 1:845af5425eec 20 PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
uchitake 1:845af5425eec 21 PID::setBias(BIAS);
uchitake 1:845af5425eec 22 PID::setMode(AUTO_MODE);
uchitake 1:845af5425eec 23 PID::setSetPoint(0.0);
uchitake 1:845af5425eec 24
uchitake 5:16ea97725085 25 wait(0.1);
uchitake 1:845af5425eec 26 HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
uchitake 5:16ea97725085 27 wait(0.1);
uchitake 1:845af5425eec 28 rawDegree = HMC6352::sample();
uchitake 5:16ea97725085 29 beforeDegree = rawDegree;
uchitake 5:16ea97725085 30 offSetDegree = rawDegree;
uchitake 5:16ea97725085 31 baseAngle = rawDegree;
uchitake 1:845af5425eec 32 initDegree = 0;
uchitake 1:845af5425eec 33 turnOverNumber = 0;
uchitake 1:845af5425eec 34 // this -> attach(this, &PIDC::updateOutput, INTERVAL);
uchitake 1:845af5425eec 35 }
uchitake 1:845af5425eec 36
uchitake 1:845af5425eec 37 PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) :
uchitake 1:845af5425eec 38 PID(kc, ti, td, interval),
uchitake 1:845af5425eec 39 HMC6352(sda, scl),
uchitake 1:845af5425eec 40 rawDegree(0),
uchitake 1:845af5425eec 41 offSetDegree(0),
uchitake 1:845af5425eec 42 turnOverNumber(0),
uchitake 1:845af5425eec 43 beforeDegree(0),
uchitake 1:845af5425eec 44 co(0),
uchitake 1:845af5425eec 45 processValue(0),
uchitake 1:845af5425eec 46 initDegree(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 5:16ea97725085 59 offSetDegree = rawDegree;
uchitake 5:16ea97725085 60 offSetDegree2 = rawDegree;
uchitake 5:16ea97725085 61 baseAngle = rawDegree;
uchitake 1:845af5425eec 62 initDegree = 0;
uchitake 1:845af5425eec 63 turnOverNumber = 0;
uchitake 5:16ea97725085 64
uchitake 1:845af5425eec 65 // this -> attach(this, &PIDC::updateOutput, INTERVAL);
uchitake 1:845af5425eec 66 }
uchitake 1:845af5425eec 67
uchitake 1:845af5425eec 68
uchitake 1:845af5425eec 69 void PIDC::confirm()
uchitake 1:845af5425eec 70 {
uchitake 1:845af5425eec 71 rawDegree = HMC6352::sample();
uchitake 1:845af5425eec 72 if(rawDegree - beforeDegree < -1800) turnOverNumber++;
uchitake 1:845af5425eec 73 if(rawDegree - beforeDegree > 1800) turnOverNumber--;
uchitake 1:845af5425eec 74 initDegree = rawDegree - offSetDegree + turnOverNumber * 3600;
uchitake 5:16ea97725085 75 initDegree2 = rawDegree - offSetDegree2 + turnOverNumber*3600;
uchitake 1:845af5425eec 76 beforeDegree = HMC6352::sample();
uchitake 1:845af5425eec 77 PID::setProcessValue(initDegree / 10.0);
uchitake 1:845af5425eec 78 co = PID::compute();
uchitake 1:845af5425eec 79 }
uchitake 1:845af5425eec 80
uchitake 1:845af5425eec 81 float PIDC::getCo() const
uchitake 1:845af5425eec 82 {
uchitake 1:845af5425eec 83 return co;
uchitake 1:845af5425eec 84 }
uchitake 1:845af5425eec 85
uchitake 5:16ea97725085 86 int PIDC::getDegree() const
uchitake 3:369d9ee17e84 87 {
uchitake 3:369d9ee17e84 88 return initDegree;
uchitake 3:369d9ee17e84 89 }
uchitake 3:369d9ee17e84 90
uchitake 5:16ea97725085 91 void PIDC::resetOffset()
uchitake 5:16ea97725085 92 {
uchitake 5:16ea97725085 93 beforeDegree = HMC6352::sample();
uchitake 5:16ea97725085 94 offSetDegree = HMC6352::sample();
uchitake 5:16ea97725085 95 turnOverNumber = 0;
uchitake 5:16ea97725085 96 }
uchitake 5:16ea97725085 97
uchitake 5:16ea97725085 98 void PIDC::resetOffset2()
uchitake 5:16ea97725085 99 {
uchitake 5:16ea97725085 100 beforeDegree = HMC6352::sample();
uchitake 5:16ea97725085 101 offSetDegree2 = HMC6352::sample();
uchitake 5:16ea97725085 102 turnOverNumber = 0;
uchitake 5:16ea97725085 103 }
uchitake 1:845af5425eec 104
uchitake 1:845af5425eec 105 void PIDC::calibration(int mode)
uchitake 1:845af5425eec 106 {
uchitake 1:845af5425eec 107 setCalibrationMode(mode);
uchitake 1:845af5425eec 108 }