Mechatronics Robotics / Mbed 2 deprecated robotV5

to compare with V6, do not change.

Dependencies:   mbed

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

Committer:
obrie829
Date:
Fri May 26 12:33:39 2017 +0000
Revision:
0:01c109e18d49
RobotV5 in working order

Who changed what in which revision?

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