zhaw_st16b_pes2_10 / Mbed 2 deprecated Pixy

n

Dependencies:   mbed

PID_Control.cpp@0:b96618c1411a, 2017-05-19 (annotated)

Committer:
beacon
Date:
Fri May 19 12:09:31 2017 +0000
Revision:
0:b96618c1411a
l

Who changed what in which revision?

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