Prometheus / PID_Control

PID_Control von Chris Henschel

PID_Control.cpp@0:e898cd8d759e, 2017-05-18 (annotated)

Committer:
ZHAW_Prometheus
Date:
Thu May 18 09:11:11 2017 +0000
Revision:
0:e898cd8d759e
PID_Control Vers. 18.05.2017

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ZHAW_Prometheus 0:e898cd8d759e 1 /*
ZHAW_Prometheus 0:e898cd8d759e 2 * PIDControl.cpp
ZHAW_Prometheus 0:e898cd8d759e 3 *
ZHAW_Prometheus 0:e898cd8d759e 4 * Created on: 16.04.2017
ZHAW_Prometheus 0:e898cd8d759e 5 * Author: chris
ZHAW_Prometheus 0:e898cd8d759e 6 */
ZHAW_Prometheus 0:e898cd8d759e 7
ZHAW_Prometheus 0:e898cd8d759e 8 #include "PID_Control.h"
ZHAW_Prometheus 0:e898cd8d759e 9
ZHAW_Prometheus 0:e898cd8d759e 10 /**
ZHAW_Prometheus 0:e898cd8d759e 11 * Constructor
ZHAW_Prometheus 0:e898cd8d759e 12 */
ZHAW_Prometheus 0:e898cd8d759e 13 PID_Control::PID_Control() :
ZHAW_Prometheus 0:e898cd8d759e 14 kp(0), ki(0), kd(0)
ZHAW_Prometheus 0:e898cd8d759e 15 {
ZHAW_Prometheus 0:e898cd8d759e 16 eOld = 0.0f;
ZHAW_Prometheus 0:e898cd8d759e 17 iSum = 0.0f;
ZHAW_Prometheus 0:e898cd8d759e 18 }
ZHAW_Prometheus 0:e898cd8d759e 19
ZHAW_Prometheus 0:e898cd8d759e 20 /**
ZHAW_Prometheus 0:e898cd8d759e 21 * Destructor
ZHAW_Prometheus 0:e898cd8d759e 22 */
ZHAW_Prometheus 0:e898cd8d759e 23 PID_Control::~PID_Control()
ZHAW_Prometheus 0:e898cd8d759e 24 {
ZHAW_Prometheus 0:e898cd8d759e 25 }
ZHAW_Prometheus 0:e898cd8d759e 26
ZHAW_Prometheus 0:e898cd8d759e 27 /**
ZHAW_Prometheus 0:e898cd8d759e 28 * Sets the PID values
ZHAW_Prometheus 0:e898cd8d759e 29 * @param p proportional gain
ZHAW_Prometheus 0:e898cd8d759e 30 * @param i integral gain
ZHAW_Prometheus 0:e898cd8d759e 31 * @param d differencial gain
ZHAW_Prometheus 0:e898cd8d759e 32 */
ZHAW_Prometheus 0:e898cd8d759e 33 void PID_Control::setPIDValues(float p, float i, float d, float _max, float _min, float _iMax)
ZHAW_Prometheus 0:e898cd8d759e 34 {
ZHAW_Prometheus 0:e898cd8d759e 35 kp = p;
ZHAW_Prometheus 0:e898cd8d759e 36 ki = i;
ZHAW_Prometheus 0:e898cd8d759e 37 kd = d;
ZHAW_Prometheus 0:e898cd8d759e 38
ZHAW_Prometheus 0:e898cd8d759e 39 max = _max;
ZHAW_Prometheus 0:e898cd8d759e 40 min = _min;
ZHAW_Prometheus 0:e898cd8d759e 41 iMax = _iMax;
ZHAW_Prometheus 0:e898cd8d759e 42 }
ZHAW_Prometheus 0:e898cd8d759e 43
ZHAW_Prometheus 0:e898cd8d759e 44 /**
ZHAW_Prometheus 0:e898cd8d759e 45 * Calculate and returns the next value from PID control
ZHAW_Prometheus 0:e898cd8d759e 46 * @param e the error
ZHAW_Prometheus 0:e898cd8d759e 47 * @param period the period
ZHAW_Prometheus 0:e898cd8d759e 48 * @return
ZHAW_Prometheus 0:e898cd8d759e 49 */
ZHAW_Prometheus 0:e898cd8d759e 50 float PID_Control::calc(float e, float period)
ZHAW_Prometheus 0:e898cd8d759e 51 {
ZHAW_Prometheus 0:e898cd8d759e 52 static float dpart = 0.0f;
ZHAW_Prometheus 0:e898cd8d759e 53 float out(0.0f);
ZHAW_Prometheus 0:e898cd8d759e 54
ZHAW_Prometheus 0:e898cd8d759e 55 iSum += e;
ZHAW_Prometheus 0:e898cd8d759e 56
ZHAW_Prometheus 0:e898cd8d759e 57 //Saturate i part
ZHAW_Prometheus 0:e898cd8d759e 58 if (iSum > iMax)
ZHAW_Prometheus 0:e898cd8d759e 59 iSum = iMax;
ZHAW_Prometheus 0:e898cd8d759e 60 if (iSum < -iMax)
ZHAW_Prometheus 0:e898cd8d759e 61 iSum = -iMax;
ZHAW_Prometheus 0:e898cd8d759e 62
ZHAW_Prometheus 0:e898cd8d759e 63 out = kp * e;
ZHAW_Prometheus 0:e898cd8d759e 64 out += ki * iSum * period;
ZHAW_Prometheus 0:e898cd8d759e 65
ZHAW_Prometheus 0:e898cd8d759e 66 dpart = 0.7f * dpart + 0.3f * (e - eOld) * 1.0f / period;
ZHAW_Prometheus 0:e898cd8d759e 67 out += kd * dpart;
ZHAW_Prometheus 0:e898cd8d759e 68
ZHAW_Prometheus 0:e898cd8d759e 69 // out += kd * (e - eOld) * 1.0f / period;
ZHAW_Prometheus 0:e898cd8d759e 70
ZHAW_Prometheus 0:e898cd8d759e 71 eOld = e;
ZHAW_Prometheus 0:e898cd8d759e 72
ZHAW_Prometheus 0:e898cd8d759e 73 if( out > max ) out = max;
ZHAW_Prometheus 0:e898cd8d759e 74 else if( out < min) out = min;
ZHAW_Prometheus 0:e898cd8d759e 75
ZHAW_Prometheus 0:e898cd8d759e 76 return out;
ZHAW_Prometheus 0:e898cd8d759e 77 }
ZHAW_Prometheus 0:e898cd8d759e 78