Keiji Akasaki / Mbed 2 deprecated PID_koushukai_20201017

aaaaaaaaaaaaaaaaaaaaa

Dependencies:   mbed QEI2

main.cpp@0:9d0c8058c974, 2020-11-19 (annotated)

Committer:
keiji0604
Date:
Thu Nov 19 11:14:04 2020 +0000
Revision:
0:9d0c8058c974
aaaaaaaaaaaaaaaaaa; ; ; ; ; ; ; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
keiji0604 0:9d0c8058c974 1 #include "mbed.h"
keiji0604 0:9d0c8058c974 2 #include "QEI.h"
keiji0604 0:9d0c8058c974 3
keiji0604 0:9d0c8058c974 4 #define Limit 30.0
keiji0604 0:9d0c8058c974 5 #define PI 3.14159265359
keiji0604 0:9d0c8058c974 6
keiji0604 0:9d0c8058c974 7 int count_r;
keiji0604 0:9d0c8058c974 8 float rev;
keiji0604 0:9d0c8058c974 9 float rev_back = 0.0;
keiji0604 0:9d0c8058c974 10 float an_vel;
keiji0604 0:9d0c8058c974 11 float powe_element = 0.0;
keiji0604 0:9d0c8058c974 12 float erorr;
keiji0604 0:9d0c8058c974 13 float erorr_back = 0.0;
keiji0604 0:9d0c8058c974 14 float goal = 2.0 * PI;
keiji0604 0:9d0c8058c974 15 float k_p = 20.0;
keiji0604 0:9d0c8058c974 16 float k_i = 3.0;
keiji0604 0:9d0c8058c974 17 float k_d = 2.5;
keiji0604 0:9d0c8058c974 18 float integ;
keiji0604 0:9d0c8058c974 19 float deriva;
keiji0604 0:9d0c8058c974 20 int power;
keiji0604 0:9d0c8058c974 21
keiji0604 0:9d0c8058c974 22 Ticker warikomi;
keiji0604 0:9d0c8058c974 23
keiji0604 0:9d0c8058c974 24 int dir;
keiji0604 0:9d0c8058c974 25 //int vel = 130;
keiji0604 0:9d0c8058c974 26
keiji0604 0:9d0c8058c974 27 Serial pc(USBTX, USBRX, 115200);
keiji0604 0:9d0c8058c974 28 Serial saber(PC_12, PD_2);
keiji0604 0:9d0c8058c974 29 Timer t;
keiji0604 0:9d0c8058c974 30 QEI motor(PC_5, PA_12, NC, 100, &t, QEI::X4_ENCODING);
keiji0604 0:9d0c8058c974 31
keiji0604 0:9d0c8058c974 32
keiji0604 0:9d0c8058c974 33 void func()
keiji0604 0:9d0c8058c974 34 {
keiji0604 0:9d0c8058c974 35 count_r = motor.getPulses();
keiji0604 0:9d0c8058c974 36 rev = (count_r / 400.0) * 2.0* PI;
keiji0604 0:9d0c8058c974 37 an_vel = (rev - rev_back) / 0.01f;
keiji0604 0:9d0c8058c974 38 rev_back = rev;
keiji0604 0:9d0c8058c974 39
keiji0604 0:9d0c8058c974 40 //////////////////////////////////
keiji0604 0:9d0c8058c974 41
keiji0604 0:9d0c8058c974 42 erorr = goal - rev;
keiji0604 0:9d0c8058c974 43 integ += (erorr + erorr_back) * (1/2) * 0.01f;
keiji0604 0:9d0c8058c974 44 deriva = (erorr - erorr_back) / 0.01f;
keiji0604 0:9d0c8058c974 45 powe_element = k_p * erorr + k_i * integ + k_d * deriva;
keiji0604 0:9d0c8058c974 46 erorr_back = erorr;
keiji0604 0:9d0c8058c974 47
keiji0604 0:9d0c8058c974 48 /////////////////////////////////////////////////////////
keiji0604 0:9d0c8058c974 49
keiji0604 0:9d0c8058c974 50 if (31.0 >= powe_element && powe_element >= 0.0) {
keiji0604 0:9d0c8058c974 51 dir = 0;
keiji0604 0:9d0c8058c974 52 } else if (powe_element < 0.0) {
keiji0604 0:9d0c8058c974 53 dir = 1;
keiji0604 0:9d0c8058c974 54 }
keiji0604 0:9d0c8058c974 55
keiji0604 0:9d0c8058c974 56 power = powe_element / 31.0f * Limit;
keiji0604 0:9d0c8058c974 57 power = (int)power;
keiji0604 0:9d0c8058c974 58
keiji0604 0:9d0c8058c974 59 if (power > 30){
keiji0604 0:9d0c8058c974 60 power = 30;
keiji0604 0:9d0c8058c974 61 }
keiji0604 0:9d0c8058c974 62
keiji0604 0:9d0c8058c974 63 /////////////////////////////////////
keiji0604 0:9d0c8058c974 64
keiji0604 0:9d0c8058c974 65 saber.putc(130);//アドレス
keiji0604 0:9d0c8058c974 66 saber.putc(dir);//回転方向
keiji0604 0:9d0c8058c974 67 saber.putc(abs(power));//出力0~127
keiji0604 0:9d0c8058c974 68 saber.putc((abs(power) + dir + 130) &0b1111111);
keiji0604 0:9d0c8058c974 69 }
keiji0604 0:9d0c8058c974 70
keiji0604 0:9d0c8058c974 71 int main()
keiji0604 0:9d0c8058c974 72 {
keiji0604 0:9d0c8058c974 73 warikomi.attach(&func, 0.01);
keiji0604 0:9d0c8058c974 74 saber.baud(115200);
keiji0604 0:9d0c8058c974 75 while(1) {
keiji0604 0:9d0c8058c974 76 pc.printf("count: %d count(float): %f rev: %f an_vel: %f powe_element: %f power: %d dir: %d erorr: %f\n"
keiji0604 0:9d0c8058c974 77 , count_r, (float)count_r, rev, an_vel, powe_element, power, dir, erorr);
keiji0604 0:9d0c8058c974 78 wait(0.01);
keiji0604 0:9d0c8058c974 79 }
keiji0604 0:9d0c8058c974 80 }