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@0:bf96e953cdb8, 2017-06-26 (annotated)
- Committer:
- echo_piyo
- Date:
- Mon Jun 26 09:59:14 2017 +0000
- Revision:
- 0:bf96e953cdb8
- Child:
- 7:c4ae1d001d09
- Child:
- 10:04f2a82cfd89
????????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
echo_piyo | 0:bf96e953cdb8 | 1 | class Position_pid { |
echo_piyo | 0:bf96e953cdb8 | 2 | public : |
echo_piyo | 0:bf96e953cdb8 | 3 | void setup(float Kp, float Ki, float Kd) { |
echo_piyo | 0:bf96e953cdb8 | 4 | kp = Kp; |
echo_piyo | 0:bf96e953cdb8 | 5 | ki = Ki; |
echo_piyo | 0:bf96e953cdb8 | 6 | kd = Kd; |
echo_piyo | 0:bf96e953cdb8 | 7 | } |
echo_piyo | 0:bf96e953cdb8 | 8 | |
echo_piyo | 0:bf96e953cdb8 | 9 | void cal(float target, float nowval, float dt) { |
echo_piyo | 0:bf96e953cdb8 | 10 | old = now; |
echo_piyo | 0:bf96e953cdb8 | 11 | now = nowval - target; |
echo_piyo | 0:bf96e953cdb8 | 12 | p = now; |
echo_piyo | 0:bf96e953cdb8 | 13 | i = i + (now + old)/2.0f * dt; |
echo_piyo | 0:bf96e953cdb8 | 14 | d = (now - old) / dt; |
echo_piyo | 0:bf96e953cdb8 | 15 | result = kp*p + ki*i + kd*d; |
echo_piyo | 0:bf96e953cdb8 | 16 | if (result > 1.0f) { |
echo_piyo | 0:bf96e953cdb8 | 17 | result = 1.0f; |
echo_piyo | 0:bf96e953cdb8 | 18 | } else if (result < -1.0f) { |
echo_piyo | 0:bf96e953cdb8 | 19 | result = -1.0f; |
echo_piyo | 0:bf96e953cdb8 | 20 | } |
echo_piyo | 0:bf96e953cdb8 | 21 | } |
echo_piyo | 0:bf96e953cdb8 | 22 | |
echo_piyo | 0:bf96e953cdb8 | 23 | float duty() { |
echo_piyo | 0:bf96e953cdb8 | 24 | return result; |
echo_piyo | 0:bf96e953cdb8 | 25 | } |
echo_piyo | 0:bf96e953cdb8 | 26 | |
echo_piyo | 0:bf96e953cdb8 | 27 | private : |
echo_piyo | 0:bf96e953cdb8 | 28 | float kp, ki, kd, |
echo_piyo | 0:bf96e953cdb8 | 29 | old, now, |
echo_piyo | 0:bf96e953cdb8 | 30 | p, i, d, result; |
echo_piyo | 0:bf96e953cdb8 | 31 | }; |
echo_piyo | 0:bf96e953cdb8 | 32 | |
echo_piyo | 0:bf96e953cdb8 | 33 | class Speed_pid { |
echo_piyo | 0:bf96e953cdb8 | 34 | public : |
echo_piyo | 0:bf96e953cdb8 | 35 | void setup(float Kp, float Ki, float Kd) { |
echo_piyo | 0:bf96e953cdb8 | 36 | kp = Kp; |
echo_piyo | 0:bf96e953cdb8 | 37 | ki = Ki; |
echo_piyo | 0:bf96e953cdb8 | 38 | kd = Kd; |
echo_piyo | 0:bf96e953cdb8 | 39 | } |
echo_piyo | 0:bf96e953cdb8 | 40 | |
echo_piyo | 0:bf96e953cdb8 | 41 | void cal(float target, float nowval, float dt) { |
echo_piyo | 0:bf96e953cdb8 | 42 | e2 = e1; |
echo_piyo | 0:bf96e953cdb8 | 43 | e1 = e; |
echo_piyo | 0:bf96e953cdb8 | 44 | e = nowval - target; |
echo_piyo | 0:bf96e953cdb8 | 45 | p = e - e1; |
echo_piyo | 0:bf96e953cdb8 | 46 | i = e*dt; |
echo_piyo | 0:bf96e953cdb8 | 47 | d = (e-2*e1+e2)/dt; |
echo_piyo | 0:bf96e953cdb8 | 48 | result = result + (kp*p+ki*i+kd*d); |
echo_piyo | 0:bf96e953cdb8 | 49 | if (result > 1.0f) { |
echo_piyo | 0:bf96e953cdb8 | 50 | result = 1.0f; |
echo_piyo | 0:bf96e953cdb8 | 51 | } else if (result < -1.0f) { |
echo_piyo | 0:bf96e953cdb8 | 52 | result = -1.0f; |
echo_piyo | 0:bf96e953cdb8 | 53 | } |
echo_piyo | 0:bf96e953cdb8 | 54 | } |
echo_piyo | 0:bf96e953cdb8 | 55 | |
echo_piyo | 0:bf96e953cdb8 | 56 | float duty() { |
echo_piyo | 0:bf96e953cdb8 | 57 | return result; |
echo_piyo | 0:bf96e953cdb8 | 58 | } |
echo_piyo | 0:bf96e953cdb8 | 59 | |
echo_piyo | 0:bf96e953cdb8 | 60 | private : |
echo_piyo | 0:bf96e953cdb8 | 61 | float kp, ki, kd, |
echo_piyo | 0:bf96e953cdb8 | 62 | e, e1, e2, |
echo_piyo | 0:bf96e953cdb8 | 63 | p, i, d, result; |
echo_piyo | 0:bf96e953cdb8 | 64 | }; |