File content as of revision 1:f26a53da33ed:

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


InterruptIn button1(D3);
AnalogIn potMeterIn(A5);
DigitalOut motor1DirectionPin(D4);
PwmOut motor1MagnitudePin(D5);

Ticker foutprinter1;
Ticker foutprinter2;
Ticker callMotor;
Serial pc(USBTX,USBRX);

volatile float motorValue=0.0;
volatile float referenceVelocity=0.0;  // in rad/s
const float maxVelocity=8.4; // in rad/s of course!
const float MotorGain=8.4; // unit: (rad/s) / PWM


void foutprint1()
{
 pc.printf("Draairichting = %i\n\r", motor1DirectionPin);
}



void foutprint2()
{
 pc.printf("Ref Vel = %f\n\r", referenceVelocity)   ;
}



float getReferenceVelocity()
{
    // Returns reference velocity in rad/s. 
    // Positive value means clockwise rotation.
    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
 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.0) motor1DirectionPin=1;
        else motor1DirectionPin=0;
    if (fabs(motorValue)>1) motor1MagnitudePin = 1;
        else motor1MagnitudePin = fabs(motorValue);       
}

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.
    referenceVelocity = getReferenceVelocity();
    motorValue = FeedForwardControl(referenceVelocity);
    SetMotor1(motorValue);
}
 
int main()
{        
motor1MagnitudePin.period(1.0);
foutprinter1.attach(foutprint1,1.0f);
foutprinter2.attach(foutprint2,1.0f);
callMotor.attach(MeasureAndControl, 0.1f);
         pc.baud(115200);
    while (true) {             
                } 
}