Cristi Stoican / Mbed 2 deprecated Migration

dsf

Dependencies:   BLE_API mbed nRF51822

PIDController.cpp@0:b5906c81772b, 2017-02-05 (annotated)

Committer:
stoicancristi
Date:
Sun Feb 05 16:31:58 2017 +0000
Revision:
0:b5906c81772b
BLE

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stoicancristi 0:b5906c81772b 1 #include "PIDController.hpp"
stoicancristi 0:b5906c81772b 2
stoicancristi 0:b5906c81772b 3 //PIDController::PIDController() {}
stoicancristi 0:b5906c81772b 4
stoicancristi 0:b5906c81772b 5 void PIDController::updateParams(ControllerParams &cp) {
stoicancristi 0:b5906c81772b 6 setKp(cp.kp);
stoicancristi 0:b5906c81772b 7 setTi(cp.ti);
stoicancristi 0:b5906c81772b 8 setTd(cp.td);
stoicancristi 0:b5906c81772b 9 }
stoicancristi 0:b5906c81772b 10
stoicancristi 0:b5906c81772b 11 float PIDController::calculateCmd() const {
stoicancristi 0:b5906c81772b 12 float outputPWM;
stoicancristi 0:b5906c81772b 13
stoicancristi 0:b5906c81772b 14 float Proportional = kp*dif(this->ref,this->out);
stoicancristi 0:b5906c81772b 15 float Integral;
stoicancristi 0:b5906c81772b 16 float Derivative = (kp*td/TE)*dif(this->ref,this->out);
stoicancristi 0:b5906c81772b 17
stoicancristi 0:b5906c81772b 18 if(ti == 0) {
stoicancristi 0:b5906c81772b 19 outputPWM = Proportional + Derivative;
stoicancristi 0:b5906c81772b 20 } else {
stoicancristi 0:b5906c81772b 21 Integral = ((kp*TE)/ti)*dif(this->ref,this->out);
stoicancristi 0:b5906c81772b 22 outputPWM = Proportional + Integral + Derivative;
stoicancristi 0:b5906c81772b 23 }
stoicancristi 0:b5906c81772b 24
stoicancristi 0:b5906c81772b 25 if(outputPWM > 1) {
stoicancristi 0:b5906c81772b 26 outputPWM = outputPWM - Integral;
stoicancristi 0:b5906c81772b 27 } else if(outputPWM < 0) {
stoicancristi 0:b5906c81772b 28 outputPWM = 0;
stoicancristi 0:b5906c81772b 29 }
stoicancristi 0:b5906c81772b 30
stoicancristi 0:b5906c81772b 31 return outputPWM;
stoicancristi 0:b5906c81772b 32 }
stoicancristi 0:b5906c81772b 33
stoicancristi 0:b5906c81772b 34 void PIDController::setKp(float _kp) {
stoicancristi 0:b5906c81772b 35 kp = _kp;
stoicancristi 0:b5906c81772b 36 }
stoicancristi 0:b5906c81772b 37
stoicancristi 0:b5906c81772b 38 void PIDController::setTi(float _ti) {
stoicancristi 0:b5906c81772b 39 ti = _ti;
stoicancristi 0:b5906c81772b 40 }
stoicancristi 0:b5906c81772b 41
stoicancristi 0:b5906c81772b 42 void PIDController::setTd(float _td) {
stoicancristi 0:b5906c81772b 43 td = _td;
stoicancristi 0:b5906c81772b 44 }
stoicancristi 0:b5906c81772b 45
stoicancristi 0:b5906c81772b 46 float PIDController::getKp() const {
stoicancristi 0:b5906c81772b 47 return kp;
stoicancristi 0:b5906c81772b 48 }
stoicancristi 0:b5906c81772b 49
stoicancristi 0:b5906c81772b 50 float PIDController::getTi() const {
stoicancristi 0:b5906c81772b 51 return ti;
stoicancristi 0:b5906c81772b 52 }
stoicancristi 0:b5906c81772b 53
stoicancristi 0:b5906c81772b 54 float PIDController::getTd() const {
stoicancristi 0:b5906c81772b 55 return td;
stoicancristi 0:b5906c81772b 56 }
stoicancristi 0:b5906c81772b 57
stoicancristi 0:b5906c81772b 58 PIDController::~PIDController() {}