Dagozilla to RoboCup / PID_buat_test_PID

Buat test_PID

Dependents:   test_PID tune_PID tune_PID

PID.cpp@0:9a32bd673e7a, 2018-03-06 (annotated)

Committer:
alienbernamaihsan
Date:
Tue Mar 06 05:05:50 2018 +0000
Revision:
0:9a32bd673e7a
Setelah take video dan boardnya rusak pas dibawa ke avrg

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alienbernamaihsan 0:9a32bd673e7a 1 #include "PID.h"
alienbernamaihsan 0:9a32bd673e7a 2
alienbernamaihsan 0:9a32bd673e7a 3
alienbernamaihsan 0:9a32bd673e7a 4 pid :: pid(double p , double i , double d , double _N , double _Ts)
alienbernamaihsan 0:9a32bd673e7a 5 {
alienbernamaihsan 0:9a32bd673e7a 6 Kp = p ; Kd = d ; Ki = i ; N = _N ; Ts = _Ts ;
alienbernamaihsan 0:9a32bd673e7a 7 a0 = (1+N*Ts);
alienbernamaihsan 0:9a32bd673e7a 8 a1 = -(2 + N*Ts);
alienbernamaihsan 0:9a32bd673e7a 9 a2 = 1;
alienbernamaihsan 0:9a32bd673e7a 10 b0 = Kp*(1+N*Ts) + Ki*Ts*(1+N*Ts) + Kd*N;
alienbernamaihsan 0:9a32bd673e7a 11 b1 = -(Kp*(2+N*Ts) + Ki*Ts + 2*Kd*N);
alienbernamaihsan 0:9a32bd673e7a 12 b2 = Kp + Kd*N;
alienbernamaihsan 0:9a32bd673e7a 13 ku1 = a1/a0;
alienbernamaihsan 0:9a32bd673e7a 14 ku2 = a2/a0;
alienbernamaihsan 0:9a32bd673e7a 15 ke0 = b0/a0;
alienbernamaihsan 0:9a32bd673e7a 16 ke1 = b1/a0;
alienbernamaihsan 0:9a32bd673e7a 17 ke2 = b2/a0;
alienbernamaihsan 0:9a32bd673e7a 18 }
alienbernamaihsan 0:9a32bd673e7a 19
alienbernamaihsan 0:9a32bd673e7a 20 double pid::createpwm( double setpoint , double feedback )
alienbernamaihsan 0:9a32bd673e7a 21 {
alienbernamaihsan 0:9a32bd673e7a 22 e2 = e1 ;
alienbernamaihsan 0:9a32bd673e7a 23 e1 = e0 ;
alienbernamaihsan 0:9a32bd673e7a 24 u2 = u1 ;
alienbernamaihsan 0:9a32bd673e7a 25 u1 = u0 ;
alienbernamaihsan 0:9a32bd673e7a 26 e0 = setpoint-feedback;
alienbernamaihsan 0:9a32bd673e7a 27 u0 = - (ku1 * u1 ) - ( ku2*u2 ) + ke0*e0 + ke1*e1 + ke2*e2;
alienbernamaihsan 0:9a32bd673e7a 28
alienbernamaihsan 0:9a32bd673e7a 29 if (u0 >= 1)
alienbernamaihsan 0:9a32bd673e7a 30 {
alienbernamaihsan 0:9a32bd673e7a 31 u0 = 1 ;
alienbernamaihsan 0:9a32bd673e7a 32 }
alienbernamaihsan 0:9a32bd673e7a 33 else if (u0 <= -1)
alienbernamaihsan 0:9a32bd673e7a 34 {
alienbernamaihsan 0:9a32bd673e7a 35 u0 = -1;
alienbernamaihsan 0:9a32bd673e7a 36 }
alienbernamaihsan 0:9a32bd673e7a 37 return u0 ;
alienbernamaihsan 0:9a32bd673e7a 38 }