![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
sampleProgram
Dependencies: QEI accelerator bit_test cyclic_io cyclic_var cylinder event_var limit mbed mecanum motor_drive pid pid_encoder rs422_put sbdbt servo
Fork of 17robo_fuzi by
Diff: pid.h
- Revision:
- 0:bf96e953cdb8
- Child:
- 7:c4ae1d001d09
- Child:
- 10:04f2a82cfd89
diff -r 000000000000 -r bf96e953cdb8 pid.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pid.h Mon Jun 26 09:59:14 2017 +0000 @@ -0,0 +1,64 @@ +class Position_pid { +public : + void setup(float Kp, float Ki, float Kd) { + kp = Kp; + ki = Ki; + kd = Kd; + } + + void cal(float target, float nowval, float dt) { + old = now; + now = nowval - target; + p = now; + i = i + (now + old)/2.0f * dt; + d = (now - old) / dt; + result = kp*p + ki*i + kd*d; + if (result > 1.0f) { + result = 1.0f; + } else if (result < -1.0f) { + result = -1.0f; + } + } + + float duty() { + return result; + } + +private : + float kp, ki, kd, + old, now, + p, i, d, result; +}; + +class Speed_pid { +public : + void setup(float Kp, float Ki, float Kd) { + kp = Kp; + ki = Ki; + kd = Kd; + } + + void cal(float target, float nowval, float dt) { + e2 = e1; + e1 = e; + e = nowval - target; + p = e - e1; + i = e*dt; + d = (e-2*e1+e2)/dt; + result = result + (kp*p+ki*i+kd*d); + if (result > 1.0f) { + result = 1.0f; + } else if (result < -1.0f) { + result = -1.0f; + } + } + + float duty() { + return result; + } + +private : + float kp, ki, kd, + e, e1, e2, + p, i, d, result; +}; \ No newline at end of file