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: headerfile_unuse/pid_.h
- Revision:
- 66:1664ee92539d
- Parent:
- 10:04f2a82cfd89
diff -r 5e4c2e5494ae -r 1664ee92539d headerfile_unuse/pid_.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/headerfile_unuse/pid_.h Sun Sep 24 05:25:03 2017 +0000 @@ -0,0 +1,65 @@ + +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