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 /**
00028  * Sets the PID values
00029  * @param p proportional gain
00030  * @param i integral gain
00031  * @param d differencial gain
00032  */
00033 void PID_Control::setPIDValues(float p, float i, float d, float _max, float _min, float _iMax)
00034 {
00035     kp = p;
00036     ki = i;
00037     kd = d;
00038 
00039     max = _max;
00040     min = _min;
00041     iMax = _iMax;
00042 }
00043 
00044 /**
00045  * Calculate and returns the next value from PID control
00046  * @param e the error
00047  * @param period the period
00048  * @return
00049  */
00050 float PID_Control::calc(float e, float period)
00051 {
00052     static float dpart = 0.0f;
00053     float out(0.0f);
00054 
00055     iSum += e;
00056 
00057     //Saturate i part
00058     if (iSum > iMax)
00059         iSum = iMax;
00060     if (iSum < -iMax)
00061         iSum = -iMax;
00062 
00063     out = kp * e;
00064     out += ki * iSum * period;
00065     
00066     dpart = 0.7f * dpart + 0.3f * (e - eOld) * 1.0f / period;
00067     out += kd * dpart;
00068 
00069    // out += kd * (e - eOld) * 1.0f / period;
00070 
00071     eOld = e;
00072 
00073     if( out > max ) out = max;
00074     else if( out < min) out = min;
00075 
00076     return out;
00077 }
00078