This is the program for 1st order LPF for velocity

Dependencies:   QEI mbed

main.cpp

Committer:
MTSAung
Date:
2018-07-20
Revision:
0:5da774f9c59c
Child:
1:4088422c9f8a

File content as of revision 0:5da774f9c59c:

#include "QEI.h"
#include "mbed.h"
Serial pc(USBTX, USBRX);

long     counts_per_rev = (1440);
double   prv_ang        = 0.0;
double   now_ang        = 0.0;
double   now_omg        = 0.0;
double   prv_time       = 0.0;
double   now_time       = 0.0;
double   samp_time      = 0.0;
double   now_x          = 0.0;
double   now_y          = 0.0;

QEI wheel (p29, p30, NC, counts_per_rev, QEI::X4_ENCODING);

float pulsesToDegrees(float pulses)
{
    return ((pulses/counts_per_rev)*360);
}

int main()
{
    Timer myTime;
    myTime.reset();
    myTime.start();
    while(1) {
        now_time  = myTime.read_ms()/1000.0;
        samp_time = now_time - prv_time;
        now_ang   = pulsesToDegrees(wheel.getPulses());
        now_omg   = (now_ang - prv_ang)/samp_time;

        pc.printf("   %F %F \r", now_time, now_ang);
        printf("\n\r");
        prv_ang  = now_ang;
        prv_time = now_time;
    }
}