Maik Overmars / 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

Fork of Robot_Software by Bram Jonkheer

help_functions/PID_controller.h

Committer:
MaikOvermars
Date:
2018-10-22
Revision:
1:ce487c9929dd
Parent:
0:ef81b9f14f58

File content as of revision 1:ce487c9929dd:

#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;
}