File content as of revision 5:931594a366b7:

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

//Defining ports
DigitalOut motor1DirectionPin (D4);
PwmOut motor1MagnitudePin(D5);
DigitalIn button(D2);
AnalogIn potmeter(A0);
Serial pc(USBTX,USBRX);
QEI encoder(D12,D13,NC,32);
HIDScope scope(1);

// Setting tickers and printers
Ticker tick;
Ticker callMotor;
Ticker pos;

const float pi = 3.14159265359;
const float ts = 1.0/1000.0;
const int velocityChannel = 0;

//Get reference velocity
float GetReferenceVelocity()
{
    // Returns reference velocity in rad/s.
    // Positive value means clockwise rotation.
    const float maxVelocity = 8.4; //Als de potmeter van 0 tot 1 gaat (als die maar tot 0.25 gaat, dan max velocity 4x zo groot als motorgain maken
    volatile float referenceVelocity;  // in rad/s
    if (button) //nog even kijken voor wanneer die + en - is
    {
        // Clockwise rotation
        referenceVelocity = potmeter * maxVelocity;
    } 
    else
    {
        // Counterclockwise rotation
        referenceVelocity = -1*potmeter * maxVelocity;
    }
    return referenceVelocity;
}

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);
    }
}

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 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);
}

volatile float radians;
volatile float velocity;
volatile float prevRadians = 0;

// position and things w/ encoder & QEI
void getPosition() {
  volatile int pulses = encoder.getPulses();
  radians = (pulses / (2 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton
  volatile float difference = radians - prevRadians;
  velocity = difference / ts;
  
  scope.set(velocityChannel,velocity);
  scope.send();
  
  prevRadians = radians;
  }
  
void print() {
    pc.printf("Motor value = %f \r\n", FeedForwardControl(GetReferenceVelocity()));
    pc.printf("Reference Velocity = %f \r\n", GetReferenceVelocity());
    pc.printf("Radians = %f \r\n", radians);
    pc.printf("Velocity = %f \r\n", velocity);
    } 
    
int main()
{
    encoder.reset();    //not entirely sure if necessary
    motor1MagnitudePin.period(1.0/100000.0);
    pos.attach(getPosition,ts);
    callMotor.attach(MeasureAndControl,1);
    pc.baud(115200);
    tick.attach(print,1);
    while(1);
}