main.cpp

00001 #include "QEI.h"
00002 #include "mbed.h"
00003 Serial pc(USBTX, USBRX);
00004 double   prv_time       = 0.0;
00005 double   now_time       = 0.0;
00006 double   samp_time      = 0.0;
00007 double   now_x          = 0.0;
00008 double   now_d          = 0.0;
00009 double   prv_d          = 0.0;
00010 double   pr2v_d         = 0.0;
00011 double   a              = 0.0;
00012 double   b              = 0.0;
00013 double   c              = 0.0;
00014 double   PI             = 3.1416;
00015 double   fc             = 15.0;
00016 double   C              = (2.0*PI*fc)*(2.0*PI*fc);
00017 double   B              = (2.0*PI*fc)*sqrt(2.0);
00018 
00019 int main()
00020 {
00021     Timer myTime;
00022     myTime.reset();
00023     myTime.start();
00024     pc.baud(57600);
00025     while(1) {
00026         now_time  = myTime.read_ms()/1000.0;
00027         samp_time = now_time - prv_time;
00028         now_x     = sin(2.0*PI*0.5*now_time) + 0.25*sin(2*PI*50.0*now_time);
00029         a         = ((samp_time*samp_time)*C)*now_x;
00030         b         = (2.0+(samp_time*B))*prv_d;
00031         c         = pr2v_d;
00032         now_d     = (a+b-c)/(((samp_time*samp_time)*C)+(samp_time*B)+1.0);
00033             
00034         pc.printf("   %F      %F\r", now_x, now_d);
00035         printf("\n\r");
00036         prv_time = now_time;
00037         prv_d    = now_d;
00038         pr2v_d   = prv_d;
00039     }
00040 }