Thijs Rakels / Mbed 2 deprecated NR_method

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:   QEI biquadFilter mbed HIDScope

PID_controler.cpp@2:f68fd7b1c655, 2018-11-01 (annotated)

Committer:
Thijsjeee
Date:
Thu Nov 01 11:13:32 2018 +0000
Revision:
2:f68fd7b1c655
Parent:
PID_controler.h@1:fafea1d00d0c
Child:
6:e492bc8fc3fb
nieuwe versie

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Thijsjeee 2:f68fd7b1c655 1 #include "PID_controler.h"
Thijsjeee 2:f68fd7b1c655 2 #include "mbed.h"
Thijsjeee 2:f68fd7b1c655 3 #include "BiQuad.h"
Thijsjeee 2:f68fd7b1c655 4 #include <math.h>
Thijsjeee 2:f68fd7b1c655 5 #include <stdio.h>
Thijsjeee 2:f68fd7b1c655 6 #include <iostream>
Thijsjeee 2:f68fd7b1c655 7 #include <stdlib.h>
Thijsjeee 2:f68fd7b1c655 8 #include <ctime>
Thijsjeee 2:f68fd7b1c655 9 #include <QEI.h>
Thijsjeee 2:f68fd7b1c655 10
Thijsjeee 1:fafea1d00d0c 11 double PID_controller(double error, double Kp, double Ki, double Kd, double Ts)
Thijsjeee 1:fafea1d00d0c 12 {
Thijsjeee 1:fafea1d00d0c 13 static double error_integral = 0;
Thijsjeee 1:fafea1d00d0c 14 static double error_prev = error;
Thijsjeee 1:fafea1d00d0c 15 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
Thijsjeee 1:fafea1d00d0c 16
Thijsjeee 1:fafea1d00d0c 17 //proportional part
Thijsjeee 1:fafea1d00d0c 18 double u_k = Kp * error;
Thijsjeee 1:fafea1d00d0c 19
Thijsjeee 1:fafea1d00d0c 20 //Integral part
Thijsjeee 1:fafea1d00d0c 21 error_integral = error_integral + error * Ts;
Thijsjeee 1:fafea1d00d0c 22 double u_i = Ki * error_integral;
Thijsjeee 1:fafea1d00d0c 23
Thijsjeee 1:fafea1d00d0c 24 // Derivative part
Thijsjeee 1:fafea1d00d0c 25 double error_derivative = (error - error_prev)/Ts;
Thijsjeee 1:fafea1d00d0c 26 double filtered_error_derivative = LowPassFilter.step(error_derivative);
Thijsjeee 1:fafea1d00d0c 27 double u_d = Kd * filtered_error_derivative;
Thijsjeee 1:fafea1d00d0c 28 error_prev = error;
Thijsjeee 2:f68fd7b1c655 29 return ((u_k + u_i + u_d)/5000);
Thijsjeee 1:fafea1d00d0c 30 }