File content as of revision 0:0e3b9c903282:

#include "mbed.h"
#include "RoboClaw.h"
#include "QEI.h"
#include "Filter.h"

Filter filter(0.01);
Timer t;
QEI motor(p21, p22, NC, 500, &t, QEI::X4_ENCODING);
RoboClaw robo(128, 115200, p9, NC);
Serial pc(USBTX, USBRX, 115200);
Ticker flipper;

int count_r;
int pre_count = 0;
int error_PID;
int goal = 3000;

double k_p = 0.00035;
double k_i = 0.00075;
double k_d = 0.0003;

double pro;
double integ;
double diff;
double s_t = 0.01;

double def;
int s_d = 0.0;

bool flag_check = false;
bool flag_printf = false;

int count_t = 0;

void func()
{
    //goal = (int)filter.SecondOrderLag(3000);
    count_t++;
    count_r = motor.getPulses();
    error_PID = (double)(goal - count_r);
    integ += (double)(count_r + pre_count) / 2.0 * s_t;
    diff = (count_r - pre_count) / s_t;
    def = k_p * error_PID + k_i * integ + k_d * diff;

    s_d = (int)def;
    if(s_d > 30) {
        s_d = 30;
    }

    if(s_d < -30) {
        s_d = -30;
    }

    flag_check = true;

    if(count_t >= 10) flag_printf = true;

    pre_count = count_r;
}

int main()
{
    filter.setSecondOrderPara(1.0, 1.0, 0.0);
    flipper.attach(&func, 0.01);

    while(1) {

        if (flag_check == true) {
            if (error_PID <= 0) {
                robo.ForwardM2(s_d);
                flag_check = false;
            } else {
                robo.BackwardM2(-1 * s_d);
                flag_check = false;
            }
        }
        if(flag_printf == true) {
            pc.printf("count_r : %d\t\terror : %d\t\ts_d : %d\t\tgoal : %d\n", count_r, error_PID, s_d, goal);
            flag_printf = false;
            count_r = 0;
        }

    }
}