File content as of revision 2:94b5e00288a5:

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

//set pins
DigitalIn encoder1A (D13); //Channel A van Encoder 1
DigitalIn encoder1B (D12); //Channel B van Encoder 1
DigitalOut led1 (D11); 
DigitalOut led2 (D10);
AnalogIn potMeter1(A0);
AnalogIn potMeter2(A1);
DigitalOut motor1DirectionPin(D7);
PwmOut motor1MagnitudePin(D6);
DigitalIn button1(D5);

//set settings
Serial pc(USBTX,USBRX);
Ticker MeasurePTicker; 
HIDScope    scope(2);

//set datatypes
int counts;
double DerivativeCounts;
int countsPrev = 0;
float referenceVelocity = 0;
double bqcDerivativeCounts = 0;
const double PI = 3.141592653589793;
double Potmeter1 = potMeter1.read();
double Potmeter2 = potMeter2.read();
const int cw = 1;
const int ccw = 0;

//define encoder counts and degrees
QEI Encoder(D12, D13, NC, 32); // turns on encoder
int counts_per_revolution = 4200;
const double gear_ratio = 131;
const double resolution = counts_per_revolution/(2*PI/gear_ratio);  //counts per radian

//set BiQuad
BiQuadChain bqc;
BiQuad bq1(0.0186,    0.0743,    0.1114,    0.0743,    0.0186); //get numbers from butter filter MATLAB
BiQuad bq2(1.0000,   -1.5704,    1.2756,   -0.4844,    0.0762);

//set go-Ticker settings
volatile bool MeasurePTicker_go=false; 
void MeasureP_act(){MeasurePTicker_go=true;}; // Activates go-flags

void MeasureP(){
    double ref_position = Potmeter1;                //reference position from potmeter
    int counts = Encoder.getPulses();           // gives position    
    double position = counts/resolution;         //position in radians
    double rotation = ref_position-position;     //rotation is 'position error' in radians
    double movement = rotation/(2*PI);  //movement in rotations
    double Kp = Potmeter2;

    double P_output = Kp*movement;
    
    if(rotation>0){
        motor1DirectionPin.write(cw);
        motor1MagnitudePin.write(P_output);
        }
    if(rotation<0){
        motor1DirectionPin.write(ccw);
        motor1MagnitudePin.write(-(P_output));
        }
    pc.printf("ref_position: %d rad/s \r\n", ref_position);
    pc.printf("position: %d rad \r\n", position);
    pc.printf("rotation: %d rad \r\n", rotation);
    pc.printf("Kp: %d \r\n", Kp);
    }

int main()
{
 //Initialize
 led1=0;
 led2=0;
 //float Potmeter1 = potMeter1.read();
 //float Potmeter2 = potMeter2.read();
 MeasurePTicker.attach(&MeasureP_act, 0.01f); 
 pc.baud(115200);  
 QEI Encoder(D12, D13, NC, 32); // turns on encoder
 
 while(1)
    {
        if (MeasurePTicker_go){
            MeasurePTicker_go=false;
            MeasureP();
        }
    }
}