File content as of revision 0:6f89ab3c1588:

#include "mbed.h"
#include "QEI.h"
//#include "mbedWSEsbc.h"

Serial pc(USBTX, USBRX);
QEI encoder(p16, p17, NC, 3200);

Ticker Tocker;
float TotalTime;
float Time;
float dt = 0.005;
float refresh = 0.01;

float theta;
float omega;
int ticks;

float theta_p;
float d_theta;

float t2d = 0.1125;

//Declare functions
void velocity();

int main()
{
    int go = 0;

    // Set serial parameters
    pc.baud(115200);
    pc.format(8,SerialBase::None,1);

    // Initialize time
    Time = 0.0;
    // Attach ticker function
    Tocker.attach(&velocity,dt);

    while(1) {
        // Get "go" command from MATLAB
        pc.scanf("%d,%f",&go, &TotalTime);
        // Reset time
        Time = 0.0;
        if(go == 1) {
            while(Time <= TotalTime) {
                //pc.printf("Time: %f; Position: %f (deg); Velocity: %f (deg/s);\n", Time, theta, omega);
                pc.printf("%f,%f,%f\n", Time, theta, omega);
                wait(refresh);
            }
        }
    }
}

void velocity()
{
    ticks = encoder.getPulses();
    theta = ticks*t2d;
    Time = Time + dt;
    // Angle in degrees

    //if (theta > 360.0) {
    //  theta = theta - 360.0;
    //} else if (theta < 0.0) {
    //  theta = theta + 360.0;
    //}

    d_theta = theta - theta_p;
    omega = d_theta/dt;

    theta_p = theta;
}