Wataru Shibazaki / Mbed 2 deprecated mbed_PID_sunple

赤崎PID プログラム

Dependencies:   mbed QEI2

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?

UserRevisionLine numberNew 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 }