File content as of revision 3:659998b2bee1:

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

HIDScope scope(3);

AnalogIn pot1(A2); // Controls potentiometer 1, which controls motor 1
AnalogIn pot2(A3); // Controls potentiometer 2, which is used to control Kp (position gain)
InterruptIn button1(D0);

InterruptIn motor1A(D13);   // Encoder 1a
InterruptIn motor1B(D12);   // Encoder 1b
QEI         Encoder1(D12,D13,NC,64);    // Note that we use X2 encoding, so there are 4200 counts/rotation, as opposed to 8400 from the documentation.

DigitalOut directionpin1(D7);
PwmOut pwmpin1(D6);
Serial pc(USBTX, USBRX); // tx, rx

Ticker Motor1;

double GetReferencePosition(){
    // Returns the wanted reference position
    const int CpR = 4200; //Counts Per Revolution; when the counts i 4200, 1 revolution has been made. (Using X2 encoding)
    double ReferencePosition = CpR*pot1; // Returns a value between 0 and 4200, the wanted rotation. 
    return ReferencePosition;
}
    
double GetActualPosition(){
    double ActualPosition = -Encoder1.getPulses(); // Gets the actual amount of counts from motor 1. Note the minus sign for convention (CCW = positive counts.)
    return ActualPosition;
}
    
double P_controller(double error){
    double Kp = pot2*17+1;
    double u_k = Kp*error;  // Proportional part
    
    // Sum all parts and return it
    return u_k;
}

void SetMotor1(double 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) directionpin1=1;
        else directionpin1=0;
    if (fabs(motorValue)>1) pwmpin1 = 1;
        else pwmpin1 = fabs(motorValue);
}

void ScopeData(){
        scope.set(0,pot1*4200);              // Wanted amount of counts
        scope.set(1,-Encoder1.getPulses());  // Amount of counts of motor 1
        scope.set(2,pwmpin1);                // Current pwm-value send to motor 1
        scope.send(); // send what's in scope memory to PC
}

void MeasureAndControl(void){
    // This function determines the desired velocity, measures the
    // actual velocity, and controls the motor with 
    // a simple Feedback controller. Call this from a Ticker.
    float ReferencePosition = GetReferencePosition();
    float ActualPosition = GetActualPosition();
    float motorValue = P_controller(ReferencePosition - ActualPosition);
    SetMotor1(motorValue);
    ScopeData();
}

int main(){
    pwmpin1.period_us(60);
  //  Motor1.attach(&MeasureAndControl, 0.01); 
    while(1){
            float ReferencePosition = GetReferencePosition();
            float ActualPosition = GetActualPosition();
            float motorValue = P_controller(ReferencePosition - ActualPosition);
            SetMotor1(motorValue);
            ScopeData();
            wait(0.01f);
        }
    }