File content as of revision 1:659489c32e75:

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



DigitalOut gpo(D0);
DigitalOut led(LED_BLUE);
DigitalOut motor1DC(D7);
DigitalOut motor1PWM(D6);
DigitalOut motor2DC(D4);
DigitalOut motor2PWM(D5);

AnalogIn   potMeterIn(A0);
DigitalIn   button1(D11);

MODSERIAL pc(USBTX,USBRX);
QEI Encoder(D12,D13,NC,32); // Input for the Encoder

Ticker controller;

float GetReferenceVelocity()
{
    // Returns reference velocity in rad/s. 
    // Positive value means clockwise rotation.
    
    const float maxVelocity=8.4; // in rad/s of course!
    
    float referenceVelocity;  // in rad/s
    
    if (button1)   {
        // Clockwise rotation
        referenceVelocity = potMeterIn * maxVelocity;
        } 
        else {
        // Counterclockwise 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
    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) motor1DC= 1;
        else motor1DC=0;
    if (fabs(motorValue)>1) motor1PWM = 1;
        else motor1PWM = fabs(motorValue);
}

void MeasureAndControl(void)
{
    float referenceVelocity = GetReferenceVelocity();
    float motorValue = FeedForwardControl(referenceVelocity);
    setMotor1(motorValue);
}

int main()
{
    pc.baud(115200);
           
    //float v = GetReferenceVelocity();
    //float controlValue = FeedForwardControl(v);
    //SetMotor1(controlValue);
    
    controller.attach(&MeasureAndControl,0.01);
    
    int counts = Encoder.getPulses();       
    pc.printf("\r `control value: %f reference velocity: %f. Motor Value is: %f number of counts: %i\n",referenceVelocity ,motorValue,counts);
    
    while(1)
    {}
}