kusano kiyoshige
first
pid.cpp@0:4ecdddd912e2, 2017-09-24 (annotated)
- Committer:
- echo_piyo
- Date:
- Sun Sep 24 05:23:58 2017 +0000
- Revision:
- 0:4ecdddd912e2
????2??
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| echo_piyo | 0:4ecdddd912e2 | 1 | #include "pid.h" |
| echo_piyo | 0:4ecdddd912e2 | 2 | |
| echo_piyo | 0:4ecdddd912e2 | 3 | void PositionPid::setup(float Kp, float Ki, float Kd, float dt){ |
| echo_piyo | 0:4ecdddd912e2 | 4 | kp = Kp; |
| echo_piyo | 0:4ecdddd912e2 | 5 | ki = Ki; |
| echo_piyo | 0:4ecdddd912e2 | 6 | kd = Kd; |
| echo_piyo | 0:4ecdddd912e2 | 7 | time = dt; |
| echo_piyo | 0:4ecdddd912e2 | 8 | frequency = 1/dt; |
| echo_piyo | 0:4ecdddd912e2 | 9 | } |
| echo_piyo | 0:4ecdddd912e2 | 10 | |
| echo_piyo | 0:4ecdddd912e2 | 11 | void PositionPid::calculate(float target, float nowValue){ |
| echo_piyo | 0:4ecdddd912e2 | 12 | old = now; |
| echo_piyo | 0:4ecdddd912e2 | 13 | now = nowValue - target; |
| echo_piyo | 0:4ecdddd912e2 | 14 | p = now; |
| echo_piyo | 0:4ecdddd912e2 | 15 | i = i + (now + old)*0.5*time; |
| echo_piyo | 0:4ecdddd912e2 | 16 | d = (now - old) * frequency; |
| echo_piyo | 0:4ecdddd912e2 | 17 | result = kp*p + ki*i + kd*d; |
| echo_piyo | 0:4ecdddd912e2 | 18 | if (result > 1.0f) { |
| echo_piyo | 0:4ecdddd912e2 | 19 | result = 1.0f; |
| echo_piyo | 0:4ecdddd912e2 | 20 | } else if (result < -1.0f) { |
| echo_piyo | 0:4ecdddd912e2 | 21 | result = -1.0f; |
| echo_piyo | 0:4ecdddd912e2 | 22 | } |
| echo_piyo | 0:4ecdddd912e2 | 23 | } |
| echo_piyo | 0:4ecdddd912e2 | 24 | |
| echo_piyo | 0:4ecdddd912e2 | 25 | float PositionPid::duty(){ |
| echo_piyo | 0:4ecdddd912e2 | 26 | return result; |
| echo_piyo | 0:4ecdddd912e2 | 27 | } |
| echo_piyo | 0:4ecdddd912e2 | 28 | |
| echo_piyo | 0:4ecdddd912e2 | 29 | float PositionPid::duty_enableWidth(float duty_min, float duty_max){ |
| echo_piyo | 0:4ecdddd912e2 | 30 | if(now > duty_min && now < duty_max){ //nowは差 |
| echo_piyo | 0:4ecdddd912e2 | 31 | return 0.0; |
| echo_piyo | 0:4ecdddd912e2 | 32 | }else{ |
| echo_piyo | 0:4ecdddd912e2 | 33 | return result; |
| echo_piyo | 0:4ecdddd912e2 | 34 | } |
| echo_piyo | 0:4ecdddd912e2 | 35 | } |
| echo_piyo | 0:4ecdddd912e2 | 36 | |
| echo_piyo | 0:4ecdddd912e2 | 37 | void SpeedPid::setup(float Kp, float Ki, float Kd, float dt){ |
| echo_piyo | 0:4ecdddd912e2 | 38 | kp = Kp; |
| echo_piyo | 0:4ecdddd912e2 | 39 | ki = Ki; |
| echo_piyo | 0:4ecdddd912e2 | 40 | kd = Kd; |
| echo_piyo | 0:4ecdddd912e2 | 41 | time = dt; |
| echo_piyo | 0:4ecdddd912e2 | 42 | frequency = 1/dt; |
| echo_piyo | 0:4ecdddd912e2 | 43 | } |
| echo_piyo | 0:4ecdddd912e2 | 44 | |
| echo_piyo | 0:4ecdddd912e2 | 45 | void SpeedPid::calculate(float target, float nowValue){ |
| echo_piyo | 0:4ecdddd912e2 | 46 | e2 = e1; |
| echo_piyo | 0:4ecdddd912e2 | 47 | e1 = e; |
| echo_piyo | 0:4ecdddd912e2 | 48 | e = nowValue - target; |
| echo_piyo | 0:4ecdddd912e2 | 49 | p = e - e1; |
| echo_piyo | 0:4ecdddd912e2 | 50 | i = e*time; |
| echo_piyo | 0:4ecdddd912e2 | 51 | d = (e-2*e1+e2)*frequency; |
| echo_piyo | 0:4ecdddd912e2 | 52 | result = result + (kp*p+ki*i+kd*d); |
| echo_piyo | 0:4ecdddd912e2 | 53 | if (result > 1.0f) { |
| echo_piyo | 0:4ecdddd912e2 | 54 | result = 1.0f; |
| echo_piyo | 0:4ecdddd912e2 | 55 | } else if (result < -1.0f) { |
| echo_piyo | 0:4ecdddd912e2 | 56 | result = -1.0f; |
| echo_piyo | 0:4ecdddd912e2 | 57 | } |
| echo_piyo | 0:4ecdddd912e2 | 58 | } |
| echo_piyo | 0:4ecdddd912e2 | 59 | |
| echo_piyo | 0:4ecdddd912e2 | 60 | float SpeedPid::duty(){ |
| echo_piyo | 0:4ecdddd912e2 | 61 | return result; |
| echo_piyo | 0:4ecdddd912e2 | 62 | } |