Biorobotics / Robot-Software_jesse

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Robot-Software by Biorobotics

help_functions/PID_controller.h

Committer:
MaikOvermars
Date:
2018-10-23
Revision:
10:7339dca7d604
Parent:
1:f1b8f0462e2a
Child:
13:3482d315877c

File content as of revision 10:7339dca7d604:

#include "mbed.h"

double Kp = 7.5;
double Ki = 1.02;
double Kd = 10;
double samplingfreq = 1000;

void PID_controller(double error1, double error2, double &u1, double &u2)
{  
    double u_k = Kp * error1;
    
    static double error_integral = 0;
    static double error_prev = error1; // initialization with this value only done once!
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    
    error_integral = error_integral + error1 * 1/samplingfreq;
    double u_i = Ki * error_integral;
    
    double error_derivative = (error1 - error_prev)*samplingfreq;
    double filtered_error_derivative = LowPassFilter.step(error_derivative);
    double u_d = Kd * filtered_error_derivative;
    error_prev = error1;
    u1 =  u_k+u_i+u_d;
}