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@51:794f160b09d4, 2017-11-18 (annotated)

Committer:
takeuchi
Date:
Sat Nov 18 17:50:59 2017 +0900
Revision:
51:794f160b09d4
Parent:
47:43f55ff8916b 
fix mode

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 47:43f55ff8916b 4 pid(KC, TI, TD, INTERVAL),
takeuchi 47:43f55ff8916b 5 r1370(PC_6, PC_7),
takeuchi 47:43f55ff8916b 6 pidcSerial(USBTX, USBRX, 115200),
takeuchi 32:b619c7787dc3 7 offSetDegree(0),
uchitake 1:845af5425eec 8 turnOverNumber(0),
uchitake 1:845af5425eec 9 beforeDegree(0),
uchitake 6:fe9767a50891 10 rawDegree(0),
uchitake 6:fe9767a50891 11 calculationResult(0),
takeuchi 32:b619c7787dc3 12 currentDegree(0)
uchitake 1:845af5425eec 13 {
takeuchi 47:43f55ff8916b 14 pid.setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
takeuchi 47:43f55ff8916b 15 pid.setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
takeuchi 47:43f55ff8916b 16 pid.setBias(BIAS);
takeuchi 47:43f55ff8916b 17 pid.setMode(AUTO_MODE);
takeuchi 47:43f55ff8916b 18 pid.setSetPoint(0.0);
uchitake 1:845af5425eec 19
uchitake 5:16ea97725085 20 wait(0.1);
takeuchi 47:43f55ff8916b 21 r1370.update();
uchitake 5:16ea97725085 22 wait(0.1);
takeuchi 47:43f55ff8916b 23 rawDegree = r1370.getRate();
uchitake 5:16ea97725085 24 beforeDegree = rawDegree;
takeuchi 32:b619c7787dc3 25 offSetDegree = rawDegree;
uchitake 1:845af5425eec 26 }
uchitake 1:845af5425eec 27
takeuchi 47:43f55ff8916b 28 PIDC::PIDC(PinName tx, PinName rx, float kc, float ti, float td, float interval) :
takeuchi 47:43f55ff8916b 29 pid(kc, ti, td, interval),
takeuchi 47:43f55ff8916b 30 r1370(tx, rx),
takeuchi 47:43f55ff8916b 31 pidcSerial(USBTX, USBRX, 115200),
takeuchi 32:b619c7787dc3 32 offSetDegree(0),
uchitake 1:845af5425eec 33 turnOverNumber(0),
uchitake 1:845af5425eec 34 beforeDegree(0),
uchitake 6:fe9767a50891 35 rawDegree(0),
uchitake 6:fe9767a50891 36 calculationResult(0),
takeuchi 32:b619c7787dc3 37 currentDegree(0)
uchitake 1:845af5425eec 38 {
takeuchi 47:43f55ff8916b 39 pid.setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
takeuchi 47:43f55ff8916b 40 pid.setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
takeuchi 47:43f55ff8916b 41 pid.setBias(BIAS);
takeuchi 47:43f55ff8916b 42 pid.setMode(AUTO_MODE);
takeuchi 47:43f55ff8916b 43 pid.setSetPoint(0.0);
uchitake 1:845af5425eec 44
takeuchi 47:43f55ff8916b 45 rawDegree = r1370.getRate();
uchitake 5:16ea97725085 46 beforeDegree = rawDegree;
takeuchi 32:b619c7787dc3 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 47:43f55ff8916b 53 r1370.update();
takeuchi 47:43f55ff8916b 54 rawDegree = r1370.getRate();
uchitake 6:fe9767a50891 55 if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber;
uchitake 6:fe9767a50891 56 if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber;
takeuchi 47:43f55ff8916b 57 currentDegree = rawDegree - offSetDegree + turnOverNumber * 360.0;
takeuchi 47:43f55ff8916b 58 r1370.update();
takeuchi 47:43f55ff8916b 59 beforeDegree = r1370.getRate();
takeuchi 47:43f55ff8916b 60 pid.setProcessValue(currentDegree);
takeuchi 47:43f55ff8916b 61 calculationResult = pid.compute();
uchitake 1:845af5425eec 62 }
uchitake 1:845af5425eec 63
uchitake 6:fe9767a50891 64 float PIDC::getCalculationResult() const
uchitake 1:845af5425eec 65 {
uchitake 6:fe9767a50891 66 return calculationResult;
uchitake 1:845af5425eec 67 }
uchitake 1:845af5425eec 68
takeuchi 47:43f55ff8916b 69 float PIDC::getCurrentDegree() const
uchitake 3:369d9ee17e84 70 {
takeuchi 32:b619c7787dc3 71 return currentDegree;
uchitake 3:369d9ee17e84 72 }
uchitake 3:369d9ee17e84 73
takeuchi 47:43f55ff8916b 74 float PIDC::getRawDegree()
uchitake 17:79fa65706f92 75 {
takeuchi 47:43f55ff8916b 76 return r1370.getRate();
uchitake 17:79fa65706f92 77 }
uchitake 17:79fa65706f92 78
takeuchi 32:b619c7787dc3 79 void PIDC::setPoint(float point)
takeuchi 32:b619c7787dc3 80 {
takeuchi 47:43f55ff8916b 81 pid.setSetPoint(point);
takeuchi 32:b619c7787dc3 82 }
takeuchi 32:b619c7787dc3 83
takeuchi 32:b619c7787dc3 84 void PIDC::resetOffset()
uchitake 5:16ea97725085 85 {
takeuchi 47:43f55ff8916b 86 beforeDegree = r1370.getRate();
takeuchi 47:43f55ff8916b 87 offSetDegree = r1370.getRate();
uchitake 5:16ea97725085 88 turnOverNumber = 0;
uchitake 5:16ea97725085 89 }