Crystal Huynh / Mbed 2 deprecated pixy_test

code for testing the pixy

Dependencies:   mbed

PID_Control.cpp@0:214d306295fa, 2017-05-30 (annotated)

Committer:
huynh270
Date:
Tue May 30 09:56:04 2017 +0000
Revision:
0:214d306295fa
hello

Who changed what in which revision?

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