Horia Carbune / Mbed 2 deprecated nRF51822_BLE_Control_Refactor

Refactoring and other updates

Dependencies:   BLE_API mbed nRF51822

Fork of nRF51822_SimpleChat by Cristi Stoican

PIDController.cpp@7:806b08205b25, 2017-05-10 (annotated)

Committer:
carbune92
Date:
Wed May 10 07:15:19 2017 +0000
Revision:
7:806b08205b25
Parent:
3:b6e4e5529a52 
fixed minor issues

Who changed what in which revision?

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