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-22
Revision:
0:4cb1de41d049
Child:
1:f1b8f0462e2a

File content as of revision 0:4cb1de41d049:

#include "mbed.h"

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

double PID_controller(double error)
{
    //Kp = potmeter2.read()*3;
    
    double u_k = Kp * error;
    
    static double error_integral = 0;
    static double error_prev = error; // initialization with this value only done once!
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    
    error_integral = error_integral + error * 1/samplingfreq;
    double u_i = Ki * error_integral;
    
    double error_derivative = (error - error_prev)*samplingfreq;
    double filtered_error_derivative = LowPassFilter.step(error_derivative);
    double u_d = Kd * filtered_error_derivative;
    error_prev = error;
    return u_k+u_i+u_d;
}