Mechatronics Robotics / Mbed 2 deprecated BrobotV1

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

Dependencies:   mbed UniServ

PID_Control.cpp@17:ec52258b9472, 2017-06-07 (annotated)

Committer:
obrie829
Date:
Wed Jun 07 11:35:59 2017 +0000
Revision:
17:ec52258b9472
Parent:
0:eba74e7a229b 
v18

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