Yolanda Tania / Mbed 2 deprecated PID_tangan

udah bisa looo

Dependencies:   mbed

pid_dagoz/PID.cpp@0:aa8e05bc0533, 2020-02-27 (annotated)

Committer:
Yolandataniaa
Date:
Thu Feb 27 12:40:03 2020 +0000
Revision:
0:aa8e05bc0533
good pid

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Yolandataniaa 0:aa8e05bc0533 1 /*
Yolandataniaa 0:aa8e05bc0533 2 * Author : Dagozilla ITB
Yolandataniaa 0:aa8e05bc0533 3 * Developer : Dagozilla ITB
Yolandataniaa 0:aa8e05bc0533 4 * Reference : https://www.scilab.org/discrete-time-pid-controller-implementation
Yolandataniaa 0:aa8e05bc0533 5 */
Yolandataniaa 0:aa8e05bc0533 6
Yolandataniaa 0:aa8e05bc0533 7 #include "PID.h"
Yolandataniaa 0:aa8e05bc0533 8
Yolandataniaa 0:aa8e05bc0533 9
Yolandataniaa 0:aa8e05bc0533 10 PID :: PID(float p , float i , float d , float _N , float _Ts, float _FF, Mode _mode)
Yolandataniaa 0:aa8e05bc0533 11 {
Yolandataniaa 0:aa8e05bc0533 12
Yolandataniaa 0:aa8e05bc0533 13 N = _N ;
Yolandataniaa 0:aa8e05bc0533 14 Ts = _Ts ;
Yolandataniaa 0:aa8e05bc0533 15 FF = _FF;
Yolandataniaa 0:aa8e05bc0533 16 mode = _mode;
Yolandataniaa 0:aa8e05bc0533 17
Yolandataniaa 0:aa8e05bc0533 18 setTunings(p, i, d);
Yolandataniaa 0:aa8e05bc0533 19
Yolandataniaa 0:aa8e05bc0533 20 ku1 = a1/a0;
Yolandataniaa 0:aa8e05bc0533 21 ku2 = a2/a0;
Yolandataniaa 0:aa8e05bc0533 22 ke0 = b0/a0;
Yolandataniaa 0:aa8e05bc0533 23 ke1 = b1/a0;
Yolandataniaa 0:aa8e05bc0533 24 ke2 = b2/a0;
Yolandataniaa 0:aa8e05bc0533 25 }
Yolandataniaa 0:aa8e05bc0533 26
Yolandataniaa 0:aa8e05bc0533 27 float PID::createpwm( float setpoint , float feedback )
Yolandataniaa 0:aa8e05bc0533 28 {
Yolandataniaa 0:aa8e05bc0533 29 e2 = e1 ;
Yolandataniaa 0:aa8e05bc0533 30 e1 = e0 ;
Yolandataniaa 0:aa8e05bc0533 31 u2 = u1 ;
Yolandataniaa 0:aa8e05bc0533 32 u1 = u0 ;
Yolandataniaa 0:aa8e05bc0533 33 e0 = setpoint-feedback;
Yolandataniaa 0:aa8e05bc0533 34 u0 = - (ku1 * u1 ) - ( ku2*u2 ) + ke0*e0 + ke1*e1 + ke2*e2 + FF*setpoint;
Yolandataniaa 0:aa8e05bc0533 35
Yolandataniaa 0:aa8e05bc0533 36 if (u0 >= 1)
Yolandataniaa 0:aa8e05bc0533 37 {
Yolandataniaa 0:aa8e05bc0533 38 u0 = 1 ;
Yolandataniaa 0:aa8e05bc0533 39 }
Yolandataniaa 0:aa8e05bc0533 40 else if (u0 <= -1)
Yolandataniaa 0:aa8e05bc0533 41 {
Yolandataniaa 0:aa8e05bc0533 42 u0 = -1;
Yolandataniaa 0:aa8e05bc0533 43 }
Yolandataniaa 0:aa8e05bc0533 44 return u0 ;
Yolandataniaa 0:aa8e05bc0533 45 }
Yolandataniaa 0:aa8e05bc0533 46
Yolandataniaa 0:aa8e05bc0533 47 void PID::setTunings(float p, float i, float d){
Yolandataniaa 0:aa8e05bc0533 48
Yolandataniaa 0:aa8e05bc0533 49 Kp = p ; Kd = d ; Ki = i ;
Yolandataniaa 0:aa8e05bc0533 50
Yolandataniaa 0:aa8e05bc0533 51 if(mode == PID_MODE){
Yolandataniaa 0:aa8e05bc0533 52 a0 = (1+N*Ts);
Yolandataniaa 0:aa8e05bc0533 53 a1 = -(2 + N*Ts);
Yolandataniaa 0:aa8e05bc0533 54 a2 = 1;
Yolandataniaa 0:aa8e05bc0533 55 b0 = Kp*(1+N*Ts) + Ki*Ts*(1+N*Ts) + Kd*N;
Yolandataniaa 0:aa8e05bc0533 56 b1 = -(Kp*(2+N*Ts) + Ki*Ts + 2*Kd*N);
Yolandataniaa 0:aa8e05bc0533 57 b2 = Kp + Kd*N;
Yolandataniaa 0:aa8e05bc0533 58
Yolandataniaa 0:aa8e05bc0533 59 }
Yolandataniaa 0:aa8e05bc0533 60 else if(mode == PI_MODE){
Yolandataniaa 0:aa8e05bc0533 61 a0 = 1;
Yolandataniaa 0:aa8e05bc0533 62 a1 = -1;
Yolandataniaa 0:aa8e05bc0533 63 a2 = 0;
Yolandataniaa 0:aa8e05bc0533 64 b0 = Kp + Ki*Ts;
Yolandataniaa 0:aa8e05bc0533 65 b1 = -Kp;
Yolandataniaa 0:aa8e05bc0533 66 b2 = 0;
Yolandataniaa 0:aa8e05bc0533 67 }
Yolandataniaa 0:aa8e05bc0533 68 }