File content as of revision 0:52ac0ff7ac4d:

#include "mbed.h"
#include "QEI.h"

#define Limit 100
#define PI 3.14159265359

int count_r;
float rev;
float rev_back = 0.0;
float an_vel;
float powe_element = 0.0;
float erorr;
float erorr_back = 0.0;
float goal = 2.0 * PI;
float k_p = 20.0;
float k_i = 3.0;
float k_d = 2.5;
float integ;
float deriva;
int power;

Ticker warikomi;

int dir;
//int vel = 130;

Serial pc(USBTX, USBRX, 115200);
RawSerial saber(p13,p14,115200);
Timer t;
QEI motor(p30, p29, NC, 100, &t, QEI::X4_ENCODING);


void func()
{
    count_r = motor.getPulses();
    rev = (count_r / 400.0) * 2.0* PI;
    an_vel = (rev - rev_back) / 0.01f;
    rev_back = rev;

    //////////////////////////////////

    erorr = goal - rev;
    integ += (erorr + erorr_back) * (1/2) * 0.01f;
    deriva = (erorr - erorr_back) / 0.01f;
    powe_element = k_p * erorr + k_i * integ + k_d * deriva;
    erorr_back = erorr;

    /////////////////////////////////////////////////////////

    if (31.0 >= powe_element && powe_element >= 0.0) {
        dir = 0;
    } else if (powe_element < 0.0) {
        dir = 1; 
    }

    power = powe_element / 31.0f * Limit;
    power = (int)power;
    
    if (power > 100){
        power = 100;
        }

    /////////////////////////////////////

    saber.putc(130);//アドレス
    saber.putc(dir);//回転方向
    saber.putc(abs(power));//出力0~127
    saber.putc((abs(power) + dir + 130) &0b1111111);
}

int main()
{
    warikomi.attach(&func, 0.01);
    saber.baud(115200);
    while(1) {
        pc.printf("count: %d  count(float): %f   rev: %f  an_vel: %f  powe_element: %f  power: %d  dir: %d  erorr: %f\n"
                  , count_r, (float)count_r, rev, an_vel, powe_element, power, dir, erorr);
                  wait(0.01);
    }
}