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@17:1f93c83e211f, 2018-10-25 (annotated)

Committer:
MaikOvermars
Date:
Thu Oct 25 22:01:12 2018 +0000
Branch:
bla
Revision:
17:1f93c83e211f
Parent:
10:7339dca7d604
Child:
21:1321ff770f99
Child:
22:31065a83d9e8
Some small bugfixes in kinematics and PID_controller and main. Added the emg processing file. In the main file the processed signals are transformed into velocities.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MaikOvermars 0:4cb1de41d049 1 #include "mbed.h"
MaikOvermars 0:4cb1de41d049 2
MaikOvermars 0:4cb1de41d049 3 double Kp = 7.5;
MaikOvermars 0:4cb1de41d049 4 double Ki = 1.02;
MaikOvermars 0:4cb1de41d049 5 double Kd = 10;
MaikOvermars 0:4cb1de41d049 6
MaikOvermars 17:1f93c83e211f 7 void PID_controller(double error1, double error2, double &u1, double &u2, const double &T)
MaikOvermars 10:7339dca7d604 8 {
MaikOvermars 10:7339dca7d604 9 double u_k = Kp * error1;
MaikOvermars 0:4cb1de41d049 10
MaikOvermars 0:4cb1de41d049 11 static double error_integral = 0;
MaikOvermars 10:7339dca7d604 12 static double error_prev = error1; // initialization with this value only done once!
MaikOvermars 0:4cb1de41d049 13 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MaikOvermars 0:4cb1de41d049 14
MaikOvermars 17:1f93c83e211f 15 error_integral = error_integral + error1 * T;
MaikOvermars 0:4cb1de41d049 16 double u_i = Ki * error_integral;
MaikOvermars 0:4cb1de41d049 17
MaikOvermars 17:1f93c83e211f 18 double error_derivative = (error1 - error_prev) / T;
MaikOvermars 0:4cb1de41d049 19 double filtered_error_derivative = LowPassFilter.step(error_derivative);
MaikOvermars 0:4cb1de41d049 20 double u_d = Kd * filtered_error_derivative;
MaikOvermars 10:7339dca7d604 21 error_prev = error1;
MaikOvermars 10:7339dca7d604 22 u1 = u_k+u_i+u_d;
MaikOvermars 0:4cb1de41d049 23 }