zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_PIXY_Official

k

Dependencies:   Servo ServoArm mbed

Sources/PID_Control.cpp@0:15a8480061e8, 2017-05-22 (annotated)

Committer:
beacon
Date:
Mon May 22 11:24:46 2017 +0000
Revision:
0:15a8480061e8
o

Who changed what in which revision?

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