File content as of revision 1:57f0db59db9f:

#include "QEI.h"
#include "mbed.h"
Serial pc(USBTX, USBRX);
double   prv_time       = 0.0;
double   now_time       = 0.0;
double   samp_time      = 0.0;
double   now_x          = 0.0;
double   now_d          = 0.0;
double   prv_d          = 0.0;
double   pr2v_d         = 0.0;
double   a              = 0.0;
double   b              = 0.0;
double   c              = 0.0;
double   PI             = 3.1416;
double   fc             = 15.0;
double   C              = (2.0*PI*fc)*(2.0*PI*fc);
double   B              = (2.0*PI*fc)*sqrt(2.0);

int main()
{
    Timer myTime;
    myTime.reset();
    myTime.start();
    pc.baud(57600);
    while(1) {
        now_time  = myTime.read_ms()/1000.0;
        samp_time = now_time - prv_time;
        now_x     = sin(2.0*PI*0.5*now_time) + 0.25*sin(2*PI*50.0*now_time);
        a         = ((samp_time*samp_time)*C)*now_x;
        b         = (2.0+(samp_time*B))*prv_d;
        c         = pr2v_d;
        now_d     = (a+b-c)/(((samp_time*samp_time)*C)+(samp_time*B)+1.0);
            
        pc.printf("   %F      %F\r", now_x, now_d);
        printf("\n\r");
        prv_time = now_time;
        prv_d    = now_d;
        pr2v_d   = prv_d;
    }
}