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