Wataru Shibazaki
/
mbed_PID_sunple
赤崎PID プログラム
main.cpp@0:52ac0ff7ac4d, 2020-11-27 (annotated)
- Committer:
- shibazakiwataru
- Date:
- Fri Nov 27 07:38:19 2020 +0000
- Revision:
- 0:52ac0ff7ac4d
AKSK
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shibazakiwataru | 0:52ac0ff7ac4d | 1 | #include "mbed.h" |
shibazakiwataru | 0:52ac0ff7ac4d | 2 | #include "QEI.h" |
shibazakiwataru | 0:52ac0ff7ac4d | 3 | |
shibazakiwataru | 0:52ac0ff7ac4d | 4 | #define Limit 100 |
shibazakiwataru | 0:52ac0ff7ac4d | 5 | #define PI 3.14159265359 |
shibazakiwataru | 0:52ac0ff7ac4d | 6 | |
shibazakiwataru | 0:52ac0ff7ac4d | 7 | int count_r; |
shibazakiwataru | 0:52ac0ff7ac4d | 8 | float rev; |
shibazakiwataru | 0:52ac0ff7ac4d | 9 | float rev_back = 0.0; |
shibazakiwataru | 0:52ac0ff7ac4d | 10 | float an_vel; |
shibazakiwataru | 0:52ac0ff7ac4d | 11 | float powe_element = 0.0; |
shibazakiwataru | 0:52ac0ff7ac4d | 12 | float erorr; |
shibazakiwataru | 0:52ac0ff7ac4d | 13 | float erorr_back = 0.0; |
shibazakiwataru | 0:52ac0ff7ac4d | 14 | float goal = 2.0 * PI; |
shibazakiwataru | 0:52ac0ff7ac4d | 15 | float k_p = 20.0; |
shibazakiwataru | 0:52ac0ff7ac4d | 16 | float k_i = 3.0; |
shibazakiwataru | 0:52ac0ff7ac4d | 17 | float k_d = 2.5; |
shibazakiwataru | 0:52ac0ff7ac4d | 18 | float integ; |
shibazakiwataru | 0:52ac0ff7ac4d | 19 | float deriva; |
shibazakiwataru | 0:52ac0ff7ac4d | 20 | int power; |
shibazakiwataru | 0:52ac0ff7ac4d | 21 | |
shibazakiwataru | 0:52ac0ff7ac4d | 22 | Ticker warikomi; |
shibazakiwataru | 0:52ac0ff7ac4d | 23 | |
shibazakiwataru | 0:52ac0ff7ac4d | 24 | int dir; |
shibazakiwataru | 0:52ac0ff7ac4d | 25 | //int vel = 130; |
shibazakiwataru | 0:52ac0ff7ac4d | 26 | |
shibazakiwataru | 0:52ac0ff7ac4d | 27 | Serial pc(USBTX, USBRX, 115200); |
shibazakiwataru | 0:52ac0ff7ac4d | 28 | RawSerial saber(p13,p14,115200); |
shibazakiwataru | 0:52ac0ff7ac4d | 29 | Timer t; |
shibazakiwataru | 0:52ac0ff7ac4d | 30 | QEI motor(p30, p29, NC, 100, &t, QEI::X4_ENCODING); |
shibazakiwataru | 0:52ac0ff7ac4d | 31 | |
shibazakiwataru | 0:52ac0ff7ac4d | 32 | |
shibazakiwataru | 0:52ac0ff7ac4d | 33 | void func() |
shibazakiwataru | 0:52ac0ff7ac4d | 34 | { |
shibazakiwataru | 0:52ac0ff7ac4d | 35 | count_r = motor.getPulses(); |
shibazakiwataru | 0:52ac0ff7ac4d | 36 | rev = (count_r / 400.0) * 2.0* PI; |
shibazakiwataru | 0:52ac0ff7ac4d | 37 | an_vel = (rev - rev_back) / 0.01f; |
shibazakiwataru | 0:52ac0ff7ac4d | 38 | rev_back = rev; |
shibazakiwataru | 0:52ac0ff7ac4d | 39 | |
shibazakiwataru | 0:52ac0ff7ac4d | 40 | ////////////////////////////////// |
shibazakiwataru | 0:52ac0ff7ac4d | 41 | |
shibazakiwataru | 0:52ac0ff7ac4d | 42 | erorr = goal - rev; |
shibazakiwataru | 0:52ac0ff7ac4d | 43 | integ += (erorr + erorr_back) * (1/2) * 0.01f; |
shibazakiwataru | 0:52ac0ff7ac4d | 44 | deriva = (erorr - erorr_back) / 0.01f; |
shibazakiwataru | 0:52ac0ff7ac4d | 45 | powe_element = k_p * erorr + k_i * integ + k_d * deriva; |
shibazakiwataru | 0:52ac0ff7ac4d | 46 | erorr_back = erorr; |
shibazakiwataru | 0:52ac0ff7ac4d | 47 | |
shibazakiwataru | 0:52ac0ff7ac4d | 48 | ///////////////////////////////////////////////////////// |
shibazakiwataru | 0:52ac0ff7ac4d | 49 | |
shibazakiwataru | 0:52ac0ff7ac4d | 50 | if (31.0 >= powe_element && powe_element >= 0.0) { |
shibazakiwataru | 0:52ac0ff7ac4d | 51 | dir = 0; |
shibazakiwataru | 0:52ac0ff7ac4d | 52 | } else if (powe_element < 0.0) { |
shibazakiwataru | 0:52ac0ff7ac4d | 53 | dir = 1; |
shibazakiwataru | 0:52ac0ff7ac4d | 54 | } |
shibazakiwataru | 0:52ac0ff7ac4d | 55 | |
shibazakiwataru | 0:52ac0ff7ac4d | 56 | power = powe_element / 31.0f * Limit; |
shibazakiwataru | 0:52ac0ff7ac4d | 57 | power = (int)power; |
shibazakiwataru | 0:52ac0ff7ac4d | 58 | |
shibazakiwataru | 0:52ac0ff7ac4d | 59 | if (power > 100){ |
shibazakiwataru | 0:52ac0ff7ac4d | 60 | power = 100; |
shibazakiwataru | 0:52ac0ff7ac4d | 61 | } |
shibazakiwataru | 0:52ac0ff7ac4d | 62 | |
shibazakiwataru | 0:52ac0ff7ac4d | 63 | ///////////////////////////////////// |
shibazakiwataru | 0:52ac0ff7ac4d | 64 | |
shibazakiwataru | 0:52ac0ff7ac4d | 65 | saber.putc(130);//アドレス |
shibazakiwataru | 0:52ac0ff7ac4d | 66 | saber.putc(dir);//回転方向 |
shibazakiwataru | 0:52ac0ff7ac4d | 67 | saber.putc(abs(power));//出力0~127 |
shibazakiwataru | 0:52ac0ff7ac4d | 68 | saber.putc((abs(power) + dir + 130) &0b1111111); |
shibazakiwataru | 0:52ac0ff7ac4d | 69 | } |
shibazakiwataru | 0:52ac0ff7ac4d | 70 | |
shibazakiwataru | 0:52ac0ff7ac4d | 71 | int main() |
shibazakiwataru | 0:52ac0ff7ac4d | 72 | { |
shibazakiwataru | 0:52ac0ff7ac4d | 73 | warikomi.attach(&func, 0.01); |
shibazakiwataru | 0:52ac0ff7ac4d | 74 | saber.baud(115200); |
shibazakiwataru | 0:52ac0ff7ac4d | 75 | while(1) { |
shibazakiwataru | 0:52ac0ff7ac4d | 76 | pc.printf("count: %d count(float): %f rev: %f an_vel: %f powe_element: %f power: %d dir: %d erorr: %f\n" |
shibazakiwataru | 0:52ac0ff7ac4d | 77 | , count_r, (float)count_r, rev, an_vel, powe_element, power, dir, erorr); |
shibazakiwataru | 0:52ac0ff7ac4d | 78 | wait(0.01); |
shibazakiwataru | 0:52ac0ff7ac4d | 79 | } |
shibazakiwataru | 0:52ac0ff7ac4d | 80 | } |