NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2017_octopus2_drive

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 R1307 PID ikarashiMDC omni_wheel

Fork of NHK2017_octopus2 by NagaokaRoboticsClub_mbedTeam

bot/PIDcontroller/PID_controller.cpp@56:e9a5cfd23833, 2017-11-28 (annotated)

Committer:
number_key
Date:
Tue Nov 28 17:54:16 2017 +0900
Revision:
56:e9a5cfd23833
Parent:
47:43f55ff8916b 
add drive5

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;
takeuchi 47:43f55ff8916b 26
takeuchi 47:43f55ff8916b 27 pidcSerial.printf("pidc OK\r\n");
uchitake 1:845af5425eec 28 }
uchitake 1:845af5425eec 29
takeuchi 47:43f55ff8916b 30 PIDC::PIDC(PinName tx, PinName rx, float kc, float ti, float td, float interval) :
takeuchi 47:43f55ff8916b 31 pid(kc, ti, td, interval),
takeuchi 47:43f55ff8916b 32 r1370(tx, rx),
takeuchi 47:43f55ff8916b 33 pidcSerial(USBTX, USBRX, 115200),
takeuchi 32:b619c7787dc3 34 offSetDegree(0),
uchitake 1:845af5425eec 35 turnOverNumber(0),
uchitake 1:845af5425eec 36 beforeDegree(0),
uchitake 6:fe9767a50891 37 rawDegree(0),
uchitake 6:fe9767a50891 38 calculationResult(0),
takeuchi 32:b619c7787dc3 39 currentDegree(0)
uchitake 1:845af5425eec 40 {
takeuchi 47:43f55ff8916b 41 pid.setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
takeuchi 47:43f55ff8916b 42 pid.setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
takeuchi 47:43f55ff8916b 43 pid.setBias(BIAS);
takeuchi 47:43f55ff8916b 44 pid.setMode(AUTO_MODE);
takeuchi 47:43f55ff8916b 45 pid.setSetPoint(0.0);
uchitake 1:845af5425eec 46
takeuchi 47:43f55ff8916b 47 rawDegree = r1370.getRate();
uchitake 5:16ea97725085 48 beforeDegree = rawDegree;
takeuchi 32:b619c7787dc3 49 offSetDegree = rawDegree;
takeuchi 47:43f55ff8916b 50 pidcSerial.printf("pidc OK\r\n");
uchitake 1:845af5425eec 51 }
uchitake 1:845af5425eec 52
uchitake 1:845af5425eec 53
uchitake 1:845af5425eec 54 void PIDC::confirm()
uchitake 1:845af5425eec 55 {
takeuchi 47:43f55ff8916b 56 r1370.update();
takeuchi 47:43f55ff8916b 57 rawDegree = r1370.getRate();
uchitake 6:fe9767a50891 58 if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber;
uchitake 6:fe9767a50891 59 if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber;
takeuchi 47:43f55ff8916b 60 currentDegree = rawDegree - offSetDegree + turnOverNumber * 360.0;
takeuchi 47:43f55ff8916b 61 r1370.update();
takeuchi 47:43f55ff8916b 62 beforeDegree = r1370.getRate();
takeuchi 47:43f55ff8916b 63 pid.setProcessValue(currentDegree);
takeuchi 47:43f55ff8916b 64 calculationResult = pid.compute();
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
takeuchi 47:43f55ff8916b 72 float PIDC::getCurrentDegree() const
uchitake 3:369d9ee17e84 73 {
takeuchi 32:b619c7787dc3 74 return currentDegree;
uchitake 3:369d9ee17e84 75 }
uchitake 3:369d9ee17e84 76
takeuchi 47:43f55ff8916b 77 float PIDC::getRawDegree()
uchitake 17:79fa65706f92 78 {
takeuchi 47:43f55ff8916b 79 return r1370.getRate();
uchitake 17:79fa65706f92 80 }
uchitake 17:79fa65706f92 81
takeuchi 32:b619c7787dc3 82 void PIDC::setPoint(float point)
takeuchi 32:b619c7787dc3 83 {
takeuchi 47:43f55ff8916b 84 pid.setSetPoint(point);
takeuchi 32:b619c7787dc3 85 }
takeuchi 32:b619c7787dc3 86
takeuchi 32:b619c7787dc3 87 void PIDC::resetOffset()
uchitake 5:16ea97725085 88 {
takeuchi 47:43f55ff8916b 89 beforeDegree = r1370.getRate();
takeuchi 47:43f55ff8916b 90 offSetDegree = r1370.getRate();
uchitake 5:16ea97725085 91 turnOverNumber = 0;
uchitake 5:16ea97725085 92 }