![](/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
pid.h
- Committer:
- echo_piyo
- Date:
- 2017-06-26
- Revision:
- 0:bf96e953cdb8
- Child:
- 7:c4ae1d001d09
- Child:
- 10:04f2a82cfd89
File content as of revision 0:bf96e953cdb8:
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; };