Hester van de Ven / Mbed 2 deprecated PIDcontroller

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@2:29daa03e4f62, 2019-10-21 (annotated)

Committer:
HestervdVen
Date:
Mon Oct 21 12:03:31 2019 +0000
Revision:
2:29daa03e4f62
Parent:
1:b862262a9d14
Child:
3:750afb8580dd
Prints inputted error

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 2:29daa03e4f62 14 float k_p = 17.5;
HestervdVen 2:29daa03e4f62 15 float k_i = 1.02;
HestervdVen 2:29daa03e4f62 16 float k_d = 23.2;
HestervdVen 2:29daa03e4f62 17 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 2:29daa03e4f62 31 cin >> a;
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 2:29daa03e4f62 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
HestervdVen 2:29daa03e4f62 66 */
HestervdVen 2:29daa03e4f62 67
RobertoO 0:67c50348f842 68
RobertoO 0:67c50348f842 69 int main()
RobertoO 0:67c50348f842 70 {
RobertoO 0:67c50348f842 71 pc.baud(115200);
HestervdVen 2:29daa03e4f62 72 pc.printf("--------\r\nWelcome!\r\n--------\r\n\n");
RobertoO 0:67c50348f842 73
HestervdVen 2:29daa03e4f62 74 startmain:
HestervdVen 2:29daa03e4f62 75 pc.printf("Please input your error.\r\n");
HestervdVen 2:29daa03e4f62 76 float er = getError();
HestervdVen 2:29daa03e4f62 77 pc.printf("Your error is: %f\r\n",er);
HestervdVen 2:29daa03e4f62 78 pc.printf("-------\r\n-------\r\n");
HestervdVen 2:29daa03e4f62 79 wait(0.5);
HestervdVen 2:29daa03e4f62 80 goto startmain;
HestervdVen 2:29daa03e4f62 81
HestervdVen 2:29daa03e4f62 82
HestervdVen 2:29daa03e4f62 83
HestervdVen 2:29daa03e4f62 84 //float outcome = PIDControl(error);
HestervdVen 2:29daa03e4f62 85 //pc.printf("The outcome is %f\n",outcome);
HestervdVen 2:29daa03e4f62 86 // pc.printf("Your input is: %f\n", error);
HestervdVen 2:29daa03e4f62 87
RobertoO 0:67c50348f842 88 }