NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated KANI2017v2

NHK2017Ateamかにろぼ

Dependencies:   2017NHKpin_config mbed FEP HMC6352 MotorDriverController PID QEI omni

classDiagram

　   ＼￣＼　　　　　　　　　　　　　    　 ／￣／ 
/l　    ＼　　＼　　 　　　　　　　　　 ／　　／　lヽ　 
|　ヽ 　ヽ　　 |　　　　 　　　　　　｜　　/　　/　| 
＼　` ‐ヽ　　ヽ　　●　　      ●    　　   /　　/　‐　 ／ 
　 ＼ __　l　　|　　｜|＿＿＿|｜ /　　l　__ ／ 
　　　 　＼　 ＼ ／　　　　　　＼／ 
　　　　　 ／＼|　　 人＿＿人　　|／＼　　　　　　 
　　　　／／＼|　　　　　      　　|／＼＼　　　　 
　　　　／／＼|　　　　　　      　|／＼＼　　　　 
　　　　／　  　 . ＼＿＿＿＿＿／　 　      ＼ 

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

bot/PIDcontroller/PID_controller.cpp@19:3a62cbc6fee9, 2017-09-04 (annotated)

Committer:
UCHITAKE
Date:
Mon Sep 04 01:01:13 2017 +0000
Revision:
19:3a62cbc6fee9
Parent:
18:41f7dd1a5ed1
Child:
22:bb6afe7332c3
add calibration

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
UCHITAKE 6:590c9622ecf1 8 PIDC::PIDC() : PID(KC, TI, TD, INTERVAL), HMC6352(HMCsda, HMCscl)
UCHITAKE 6:590c9622ecf1 9 {
uchitake 18:41f7dd1a5ed1 10 PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
uchitake 18:41f7dd1a5ed1 11 PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
uchitake 18:41f7dd1a5ed1 12 PID::setBias(BIAS);
uchitake 18:41f7dd1a5ed1 13 PID::setMode(AUTO_MODE);
uchitake 18:41f7dd1a5ed1 14 PID::setSetPoint(0.0);
UCHITAKE 6:590c9622ecf1 15
uchitake 18:41f7dd1a5ed1 16 HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
uchitake 18:41f7dd1a5ed1 17 rawDegree = HMC6352::sample();
uchitake 18:41f7dd1a5ed1 18 beforeDegree = HMC6352::sample();
uchitake 18:41f7dd1a5ed1 19 offSetDegree = HMC6352::sample();
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 18:41f7dd1a5ed1 25 PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) :
UCHITAKE 19:3a62cbc6fee9 26 PID(kc, ti, td, interval), HMC6352(sda, scl)
UCHITAKE 19:3a62cbc6fee9 27 {
UCHITAKE 19:3a62cbc6fee9 28 PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
UCHITAKE 19:3a62cbc6fee9 29 PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
UCHITAKE 19:3a62cbc6fee9 30 PID::setBias(BIAS);
UCHITAKE 19:3a62cbc6fee9 31 PID::setMode(AUTO_MODE);
UCHITAKE 19:3a62cbc6fee9 32 PID::setSetPoint(0.0);
UCHITAKE 19:3a62cbc6fee9 33
UCHITAKE 19:3a62cbc6fee9 34 HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
UCHITAKE 19:3a62cbc6fee9 35 rawDegree = HMC6352::sample();
UCHITAKE 19:3a62cbc6fee9 36 beforeDegree = HMC6352::sample();
UCHITAKE 19:3a62cbc6fee9 37 offSetDegree = HMC6352::sample();
UCHITAKE 19:3a62cbc6fee9 38 initDegree = 0;
UCHITAKE 19:3a62cbc6fee9 39 turnOverNumber = 0;
UCHITAKE 19:3a62cbc6fee9 40 // this -> attach(this, &PIDC::updateOutput, INTERVAL);
UCHITAKE 19:3a62cbc6fee9 41 }
uchitake 18:41f7dd1a5ed1 42
uchitake 18:41f7dd1a5ed1 43
UCHITAKE 6:590c9622ecf1 44 void PIDC::confirm()
UCHITAKE 6:590c9622ecf1 45 {
uchitake 18:41f7dd1a5ed1 46 rawDegree = HMC6352::sample();
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;
uchitake 18:41f7dd1a5ed1 50 beforeDegree = HMC6352::sample();
uchitake 18:41f7dd1a5ed1 51 PID::setProcessValue(initDegree / 10.0);
uchitake 18: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 18:41f7dd1a5ed1 58 }
UCHITAKE 19:3a62cbc6fee9 59
UCHITAKE 19:3a62cbc6fee9 60
UCHITAKE 19:3a62cbc6fee9 61 void PIDC::calibration(int mode)
UCHITAKE 19:3a62cbc6fee9 62 {
UCHITAKE 19:3a62cbc6fee9 63 setCalibrationMode(mode);
UCHITAKE 19:3a62cbc6fee9 64 }