File content as of revision 0:35f03aa18046:

#include "mbed.h"
#include "math.h"
#include "HIDScope.h"
#include "QEI.h"

// ------ Hardware Interfaces -------

const PinName motor1dir = D7;
const PinName motor1PWM = D6;
DigitalOut motor1direction(motor1dir);
PwmOut motor1control(motor1PWM);

const PinName button1name = D10; //for some reason D8 does not seem to work. wtf!
const PinName pot1name = A0;
InterruptIn button1(button1name);
AnalogIn potMeterIn(pot1name);

const PinName encoderpin1 = D13;
const PinName encoderpin2 = D12;
QEI qei(encoderpin1, encoderpin2, NC, 32);

// ------- Objects used -------

// Ticker which controls the mother every 1/100 of a second.
Ticker controlTicker;

Ticker debugTicker;
Serial pc(USBTX, USBRX);

Ticker encoderTicker;

HIDScope scope(1);

// ------- Constants

const float motorGain = 8.4f;

const float MAX_VELOCITY = 8.4f; //radians per second

const bool CLOCKWISE = true;

//1 output rod revolution == 131.25 encoder revolution
const float RATIO = 1.0f/131.25f;

const float tSample = 1.0f/1000.0f;

const double pi = 3.14159265358979323846;

const int VELOCITY_CHANNEL = 0;


// ------ variables

volatile bool direction = CLOCKWISE;

volatile double lastEncoderRead = 0; //angle in radians.

// ------ functions

float getReferenceVelocity() {
    // Returns reference velocity in rad/s. 
    return MAX_VELOCITY * potMeterIn.read();
}

void setMotor1(float motorValue) {
    // Given motorValue<=1, writes the velocity to the pwm control.
    // MotorValues outside range are truncated to within range.
    motor1control.write(fabs(motorValue) > 1 ? 1 : fabs(motorValue));
}

float feedForwardControl(float referenceVelocity) {
    // very simple linear feed-forward control
    // returns motorValue
    return referenceVelocity / motorGain;
}

void measureAndControl(void) {
    // This function measures the potmeter position, extracts a
    // reference velocity from it, and controls the motor with 
    // a simple FeedForward controller. Call this from a Ticker.
    float referenceVelocity = getReferenceVelocity();
    float motorValue = feedForwardControl(referenceVelocity);
    setMotor1(motorValue);
}

void onButtonPress() {
    // reverses the direction
    motor1direction.write(direction = !direction);
    pc.printf("direction: %s\r\n\n", direction == CLOCKWISE ? "clockwise" : "counter clockwise");
}

void onDebugTick() {
    
    pc.printf("pot input: %f\r\n", potMeterIn.read());
    pc.printf("motorValue: %f\r\n", feedForwardControl(getReferenceVelocity()));
    pc.printf("\n\n\n");

}

void encoderTick() {
    int pulses = qei.getPulses();
    
    //calculate the total angle that is travelel so far.
    double radians = (pulses / tSample) * ((2*pi)/32);
    
    //approximate the derivative for the angular velocity.
    double xdif = radians - lastEncoderRead;
    double xderiv = xdif / tSample;
    
    //TODO apply a BiQuard filter in between here.
    
    //send velocity to HIDScope
    scope.set(VELOCITY_CHANNEL, xderiv);
    scope.send();
    
    //update lastEncoderRead such that it can be used for next time
    lastEncoderRead = radians;
}

int main()
{
    pc.baud(115200);
    qei.reset();
    
    encoderTicker.attach(&encoderTick, tSample);
    controlTicker.attach(&measureAndControl, 1.0f/100.0f);
    button1.fall(&onButtonPress);
    
    debugTicker.attach(&onDebugTick, 1);
    
    while (true);
}