Mechatronics Robotics / Mbed 2 deprecated BrobotV1

These are the core files for the Robot at Team conception.

Dependencies:   mbed UniServ

PID_Control.cpp@0:eba74e7a229b, 2017-05-26 (annotated)

Committer:
obrie829
Date:
Fri May 26 07:40:48 2017 +0000
Revision:
0:eba74e7a229b
This is the first version committed to team Brobot and should have all header and class files contained within

Who changed what in which revision?

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