Biorobotics / Robot-Software

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed Servo

help_functions/PID_controller.h

Committer:
MaikOvermars
Date:
2018-10-25
Branch:
bla
Revision:
17:1f93c83e211f
Parent:
10:7339dca7d604
Child:
21:1321ff770f99
Child:
22:31065a83d9e8

File content as of revision 17:1f93c83e211f:

#include "mbed.h"

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

void PID_controller(double error1, double error2, double &u1, double &u2, const double &T)
{  
    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 * T;
    double u_i = Ki * error_integral;
    
    double error_derivative = (error1 - error_prev) / T;
    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;
}