Hester van de Ven / Mbed 2 deprecated PIDcontroller

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@3:750afb8580dd, 2019-10-21 (annotated)

Committer:
HestervdVen
Date:
Mon Oct 21 12:17:59 2019 +0000
Revision:
3:750afb8580dd
Parent:
2:29daa03e4f62
Child:
4:f6862a157db9
PID controller werkt met input error, maar moet nog getweaked worden

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
RobertoO 1:b862262a9d14 2 #include "MODSERIAL.h"
HestervdVen 2:29daa03e4f62 3 #include "BiQuad.h"
HestervdVen 2:29daa03e4f62 4 #include <iostream>
HestervdVen 2:29daa03e4f62 5 #include "FastPWM.h"
HestervdVen 2:29daa03e4f62 6 #include "QEI.h"
HestervdVen 2:29daa03e4f62 7 #include <math.h>
HestervdVen 2:29daa03e4f62 8 //#include "HIDScope.h"
HestervdVen 2:29daa03e4f62 9
HestervdVen 2:29daa03e4f62 10
HestervdVen 2:29daa03e4f62 11 Serial pc(USBTX, USBRX);
HestervdVen 2:29daa03e4f62 12 volatile float width_percent;
HestervdVen 2:29daa03e4f62 13
HestervdVen 3:750afb8580dd 14 const float k_p = 3;
HestervdVen 3:750afb8580dd 15 const float k_i = 1000;
HestervdVen 3:750afb8580dd 16 const float k_d = 1;
HestervdVen 3:750afb8580dd 17 const float t_s = 0.0025; //sample time in sec
HestervdVen 2:29daa03e4f62 18
HestervdVen 2:29daa03e4f62 19
HestervdVen 2:29daa03e4f62 20 //inputs for LPF; still to fill in!
HestervdVen 2:29daa03e4f62 21 float b1;
HestervdVen 2:29daa03e4f62 22 float b2;
HestervdVen 2:29daa03e4f62 23 float b3;
HestervdVen 2:29daa03e4f62 24 float b4;
HestervdVen 2:29daa03e4f62 25 float b5;
HestervdVen 2:29daa03e4f62 26
HestervdVen 2:29daa03e4f62 27
RobertoO 0:67c50348f842 28
HestervdVen 2:29daa03e4f62 29 float getError(){
HestervdVen 2:29daa03e4f62 30 float a; //error
HestervdVen 3:750afb8580dd 31 cin >> a; //input on keyboard
HestervdVen 2:29daa03e4f62 32 return a;
HestervdVen 2:29daa03e4f62 33 }
HestervdVen 2:29daa03e4f62 34
HestervdVen 2:29daa03e4f62 35
HestervdVen 2:29daa03e4f62 36 /*PID controller code
HestervdVen 2:29daa03e4f62 37 Proportional part general formula: u_k = k_p * e
HestervdVen 2:29daa03e4f62 38 Integral part general formula: u_i = k_i * integral e dt
HestervdVen 2:29daa03e4f62 39 Derivative part general formula: u_d = k_d * derivative e */
HestervdVen 2:29daa03e4f62 40
HestervdVen 2:29daa03e4f62 41
RobertoO 0:67c50348f842 42
HestervdVen 3:750afb8580dd 43 float PIDControl(float error){
HestervdVen 2:29daa03e4f62 44 static float error_integral = 0;
HestervdVen 2:29daa03e4f62 45 static float error_prev = error;
HestervdVen 2:29daa03e4f62 46 static BiQuad LPF (b1, b2, b3, b4, b5);
HestervdVen 2:29daa03e4f62 47
HestervdVen 2:29daa03e4f62 48 //proportional
HestervdVen 2:29daa03e4f62 49 float u_k = k_p * error;
HestervdVen 2:29daa03e4f62 50
HestervdVen 2:29daa03e4f62 51 //integral
HestervdVen 2:29daa03e4f62 52 error_integral = error_integral + error * t_s;
HestervdVen 2:29daa03e4f62 53 float u_i = k_i * error_integral;
HestervdVen 2:29daa03e4f62 54
HestervdVen 2:29daa03e4f62 55 //differential
HestervdVen 2:29daa03e4f62 56 float error_derivative = (error - error_prev) / t_s;
HestervdVen 2:29daa03e4f62 57 // float filtered_error = LPF.step(error_derivative); //LPF with function of BiQuad library
HestervdVen 2:29daa03e4f62 58 // float u_d = k_d * filtered_error;
HestervdVen 2:29daa03e4f62 59 float u_d = k_d * error_derivative; //this is very sensitive to noise, hence the LPF above
HestervdVen 2:29daa03e4f62 60 error_prev = error;
HestervdVen 2:29daa03e4f62 61
HestervdVen 2:29daa03e4f62 62 return u_k + u_i + u_d;
HestervdVen 2:29daa03e4f62 63 }
HestervdVen 2:29daa03e4f62 64
HestervdVen 2:29daa03e4f62 65
RobertoO 0:67c50348f842 66 int main()
RobertoO 0:67c50348f842 67 {
RobertoO 0:67c50348f842 68 pc.baud(115200);
HestervdVen 2:29daa03e4f62 69 pc.printf("--------\r\nWelcome!\r\n--------\r\n\n");
RobertoO 0:67c50348f842 70
HestervdVen 2:29daa03e4f62 71 startmain:
HestervdVen 2:29daa03e4f62 72 pc.printf("Please input your error.\r\n");
HestervdVen 2:29daa03e4f62 73 float er = getError();
HestervdVen 2:29daa03e4f62 74 pc.printf("Your error is: %f\r\n",er);
HestervdVen 2:29daa03e4f62 75 pc.printf("-------\r\n-------\r\n");
HestervdVen 2:29daa03e4f62 76 wait(0.5);
HestervdVen 3:750afb8580dd 77
HestervdVen 2:29daa03e4f62 78
HestervdVen 2:29daa03e4f62 79
HestervdVen 2:29daa03e4f62 80
HestervdVen 3:750afb8580dd 81 float outcome = PIDControl(er);
HestervdVen 3:750afb8580dd 82 pc.printf("The outcome is %f\n",outcome);
HestervdVen 3:750afb8580dd 83
HestervdVen 3:750afb8580dd 84
HestervdVen 3:750afb8580dd 85 goto startmain; //allows to input multiple numbers after each other
HestervdVen 2:29daa03e4f62 86
RobertoO 0:67c50348f842 87 }