Revision 0:9a32bd673e7a, committed 2018-03-06

Comitter:

alienbernamaihsan

Date:

Tue Mar 06 05:05:50 2018 +0000

Commit message:

Setelah take video dan boardnya rusak pas dibawa ke avrg

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Tue Mar 06 05:05:50 2018 +0000
@@ -0,0 +1,38 @@
+#include "PID.h"
+
+
+pid :: pid(double p , double i , double d , double _N , double _Ts)
+{
+    Kp = p ; Kd = d ; Ki = i ; N = _N ; Ts = _Ts ;
+    a0 = (1+N*Ts);
+    a1 = -(2 + N*Ts);
+    a2 = 1;
+    b0 = Kp*(1+N*Ts) + Ki*Ts*(1+N*Ts) + Kd*N;
+    b1 = -(Kp*(2+N*Ts) + Ki*Ts + 2*Kd*N);
+    b2 = Kp + Kd*N;
+    ku1 = a1/a0;
+    ku2 = a2/a0;
+    ke0 = b0/a0;
+    ke1 = b1/a0;
+    ke2 = b2/a0;
+}
+
+double pid::createpwm( double setpoint , double feedback )
+{
+    e2 = e1 ;
+    e1 = e0 ;
+    u2 = u1 ;
+    u1 = u0 ;
+    e0 = setpoint-feedback;
+    u0 = - (ku1 * u1 )  - ( ku2*u2 )  + ke0*e0 + ke1*e1 + ke2*e2;
+
+    if (u0 >= 1)
+    {
+        u0 = 1 ;
+    }
+    else if (u0 <= -1)
+    {
+        u0 = -1;
+    }
+    return u0 ;   
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.h	Tue Mar 06 05:05:50 2018 +0000
@@ -0,0 +1,38 @@
+#ifndef PID_H
+#define PID_H
+
+#include "mbed.h"
+
+class pid
+{
+public :
+        pid(double p , double i , double d , double _N , double _Ts) ;
+        double createpwm( double setpoint , double feedback ) ;
+    
+private :
+        double Kp ;
+        double Kd ;
+        double Ki ;
+        double N ;
+        double Ts ;
+        double a0;
+        double a1;
+        double a2;
+        double b0;
+        double b1;
+        double b2;
+        double ku1;
+        double ku2;
+        double ke0;
+        double ke1;
+        double ke2;
+        double e2;
+        double e1;
+        double e0;
+        double u2;
+        double u1;
+        double u0; 
+};
+#endif
+
+    
\ No newline at end of file