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@1:ce487c9929dd, 2018-10-22 (annotated)

Committer:
MaikOvermars
Date:
Mon Oct 22 11:10:41 2018 +0000
Revision:
1:ce487c9929dd
Parent:
0:ef81b9f14f58 
Updated some code and started implementing kinematics.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bjonkheer 0:ef81b9f14f58 1 #include "mbed.h"
bjonkheer 0:ef81b9f14f58 2
MaikOvermars 1:ce487c9929dd 3 double Kp = 7.5;
MaikOvermars 1:ce487c9929dd 4 double Ki = 1.02;
MaikOvermars 1:ce487c9929dd 5 double Kd = 10;
MaikOvermars 1:ce487c9929dd 6 double samplingfreq = 1000;
MaikOvermars 1:ce487c9929dd 7
bjonkheer 0:ef81b9f14f58 8 double PID_controller(double error)
bjonkheer 0:ef81b9f14f58 9 {
MaikOvermars 1:ce487c9929dd 10 //Kp = potmeter2.read()*3;
bjonkheer 0:ef81b9f14f58 11
bjonkheer 0:ef81b9f14f58 12 double u_k = Kp * error;
bjonkheer 0:ef81b9f14f58 13
bjonkheer 0:ef81b9f14f58 14 static double error_integral = 0;
bjonkheer 0:ef81b9f14f58 15 static double error_prev = error; // initialization with this value only done once!
bjonkheer 0:ef81b9f14f58 16 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
bjonkheer 0:ef81b9f14f58 17
bjonkheer 0:ef81b9f14f58 18 error_integral = error_integral + error * 1/samplingfreq;
bjonkheer 0:ef81b9f14f58 19 double u_i = Ki * error_integral;
bjonkheer 0:ef81b9f14f58 20
bjonkheer 0:ef81b9f14f58 21 double error_derivative = (error - error_prev)*samplingfreq;
bjonkheer 0:ef81b9f14f58 22 double filtered_error_derivative = LowPassFilter.step(error_derivative);
bjonkheer 0:ef81b9f14f58 23 double u_d = Kd * filtered_error_derivative;
bjonkheer 0:ef81b9f14f58 24 error_prev = error;
bjonkheer 0:ef81b9f14f58 25 return u_k+u_i+u_d;
bjonkheer 0:ef81b9f14f58 26 }