PID_controler.cpp

00001 #include "PID_controler.h"
00002 #include "mbed.h"
00003 #include "BiQuad.h"
00004 #include <math.h>
00005 #include <stdio.h>
00006 #include <iostream>
00007 #include <stdlib.h>
00008 #include <ctime>1
00009 #include <QEI.h>
00010 
00011 double PID_controller(double error, double Kp, double Ki, double Kd, double Ts)
00012 {
00013     static double error_integral = 0;
00014     static double error_prev = error;
00015     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
00016 
00017     //proportional part
00018     double u_k = Kp * error;
00019 
00020     //Integral part
00021     error_integral = error_integral + error * Ts;
00022     double u_i = Ki * error_integral;
00023 
00024     // Derivative part
00025     double error_derivative = (error - error_prev)/Ts;
00026     double filtered_error_derivative = LowPassFilter.step(error_derivative);
00027     double u_d = Kd * filtered_error_derivative;
00028     error_prev = error;
00029     return ((u_k + u_i + u_d)/5000);
00030 }