File content as of revision 7:ec9ce9b90006:

#include "mbed.h"
#include <math.h>
#include "MODSERIAL.h"
#include "QEI.h"
#include "HIDScope.h"

DigitalIn encoder1A (D13); //Channel A van Encoder 1
DigitalIn encoder1B (D12); //Channel B van Encoder 1
DigitalOut led1 (D11); 
DigitalOut led2 (D10);
AnalogIn potMeterIn(PTB2);
DigitalIn button1(D5);
DigitalOut motor1DirectionPin(D7);
PwmOut motor1MagnitudePin(D6);

Serial pc(USBTX,USBRX);
Ticker MeasureTicker, TimeTracker, sampleT;
HIDScope    scope(2);

float referenceVelocity = 0;
volatile int counts;
volatile float DerivativeCounts;
volatile int countsPrev = 0;

volatile bool MeasureTicker_go=false, TimeTracker_go=false, sampleT_go=false;

void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
void TimeTracker_act(){TimeTracker_go=true;};
void sampleT_act(){sampleT_go=true;};

float GetReferenceVelocity()
{
    // Returns reference velocity in rad/s. 
    // Positive value means clockwise rotation.
    const float maxVelocity = 8.4; // in rad/s of course!
    if (button1 == 0){
        led1=1;
        led2=0;
        // Counterclockwise rotation
        referenceVelocity = potMeterIn * maxVelocity;  
    } 
    else {   
        led1=0;
        led2=1;
        // Clockwise rotation
        referenceVelocity = -1*potMeterIn * maxVelocity;
    }
    return referenceVelocity;
}

float FeedForwardControl(float referenceVelocity)
{
    // very simple linear feed-forward control
    const float MotorGain=8.4; // unit: (rad/s) / PWM
    volatile float motorValue = referenceVelocity / MotorGain;
    return motorValue;
}

void SetMotor1(float motorValue)
{
    // Given -1<=motorValue<=1, this sets the PWM and direction
    // bits for motor 1. Positive value makes motor rotating
    // clockwise. motorValues outside range are truncated to
    // within range
    if (motorValue >=0) motor1DirectionPin=1;
        else motor1DirectionPin=0;
    if (fabs(motorValue)>1) motor1MagnitudePin = 1;
        else motor1MagnitudePin = fabs(motorValue);
}

void EncoderHIDScope() // encoder in HIDScope zetten
{
    QEI Encoder(D12, D13, NC, 32);
    counts = Encoder.getPulses();  // gives position
    scope.set(0, counts);
    DerivativeCounts = (counts-countsPrev)/0.001;
    scope.set(1, DerivativeCounts);
    countsPrev = counts; 
    scope.send();
}

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.
    volatile float referenceVelocity = GetReferenceVelocity();
    volatile float motorValue = FeedForwardControl(referenceVelocity);
    SetMotor1(motorValue);
    EncoderHIDScope();
    //pc.printf("MotorValue: %f rad/s \r\n", motorValue);
}

void TimeTrackerF(){
     wait(1);   
     float Potmeter = potMeterIn.read();
     pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
     pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
     //pc.printf("MotorValue: %f rad/s \r\n", motorValue);  
     pc.printf("Encoder counts: %i \r\n", countsPrev);
}

int main()
{
 //Initialize
 led1=0;
 led2=0;
 counts;
 //float Potmeter = potMeterIn.read();
 MeasureTicker.attach(&MeasureTicker_act, 0.01f); 
 TimeTracker.attach(&TimeTracker_act, 0.3f);
 pc.baud(115200);
 QEI Encoder(D12, D13, NC, 32); // turns on encoder
 sampleT.attach(&sampleT_act, 0.001f);
 //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
 //pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
 
 while(1)
    {
        
        if (MeasureTicker_go){
            MeasureTicker_go=false;
            MeasureAndControl();
            //counts = Encoder.getPulses();  // gives position
            //pc.printf("Encoder counts: %i \r\n", counts); 
        }
        if (TimeTracker_go){
            TimeTracker_go=false;
            TimeTrackerF();
        }
        if (sampleT_go){
            sampleT_go=false;
            EncoderHIDScope();
        }
    }
}