PID_Control.cpp

00001 /*
00002  * PIDControl.cpp
00003  *
00004  *  Created on: 16.04.2017
00005  *      Author: chris
00006  */
00007 
00008 #include "PID_Control.h"
00009 
00010 /**
00011  * Constructor
00012  */
00013 PID_Control::PID_Control() :
00014     kp(0), ki(0), kd(0)
00015 {
00016     eOld = 0.0f;
00017     iSum = 0.0f;
00018 }
00019 
00020 /**
00021  * Destructor
00022  */
00023 PID_Control::~PID_Control(){
00024 }
00025 
00026 /**
00027  * Sets the PID values
00028  * @param p proportional gain
00029  * @param i integral gain
00030  * @param d differencial gain
00031  */
00032 void PID_Control::setPIDValues(float p, float i, float d, float _max, float _min, float _iMax)
00033 {
00034     kp = p;
00035     ki = i;
00036     kd = d;
00037 
00038     max = _max;
00039     min = _min;
00040     iMax = _iMax;
00041 }
00042 
00043 /**
00044  * Calculate and returns the next value from PID control
00045  * @param e the error
00046  * @param period the period
00047  * @return
00048  */
00049 float PID_Control::calc(float e, float period)
00050 {
00051     static float dpart = 0.0f;
00052     float out(0.0f);
00053 
00054     iSum += e;
00055 
00056     //Saturate i part
00057     if (iSum > iMax)
00058         iSum = iMax;
00059     if (iSum < -iMax)
00060         iSum = -iMax;
00061 
00062     out = kp * e;
00063     out += ki * iSum * period;
00064     
00065     dpart = 0.7f * dpart + 0.3f * (e - eOld) * 1.0f / period;  //low pass filter
00066     out += kd * dpart;
00067 
00068    // out += kd * (e - eOld) * 1.0f / period;  // is affected by noise
00069 
00070     eOld = e;
00071 
00072     if( out > max ) out = max;
00073     else if( out < min) out = min;
00074 
00075     return out;
00076 }
00077