![](/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@7:c4ae1d001d09, 2017-07-01 (annotated)
- Committer:
- echo_piyo
- Date:
- Sat Jul 01 10:12:46 2017 +0000
- Revision:
- 7:c4ae1d001d09
- Parent:
- 0:bf96e953cdb8
rotaryencoder?rad???????
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 | }; |