File content as of revision 0:85ca550760e9:

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



DigitalOut gpo(D0);
DigitalOut ledb(LED_BLUE);
DigitalOut ledr(LED_RED);
DigitalOut ledg(LED_GREEN);
DigitalOut motor1DC(D7);
DigitalOut motor1PWM(D6);
DigitalOut motor2DC(D4);
DigitalOut motor2PWM(D5);

AnalogIn   potMeter1(A0);
AnalogIn   potMeter2(A1);
DigitalIn  button1(D11);
DigitalIn  button2(D12);
QEI Encoder1(D12,D13,NC,4200);

MODSERIAL pc(USBTX,USBRX);

Ticker controller;

const double pi = 3.1415926535897;
const float MOTOR1_KP = 5;
const float RAD_PER_PULSE = (2*pi)/4200;
const float CONTROLLER_TS = 0.01;     //TIME INTERVAL/ hZ

float P(double error, const float Kp){
    return Kp * error;
    }

void motor1_Controller(){
    double reference = 10*potMeter1.read();
    double position = RAD_PER_PULSE*Encoder1.getPulses();
    double motor1 = P(reference-position, MOTOR1_KP);
    motor1PWM = motor1;
    
    if(motor1 > 0.1){
        motor1DC = 1;
        
        ledr = 1;
        ledg = 1;       //Blau
        ledb = 0;
    }
    else if (motor1<-0.1) {
        motor1DC = 0; 
        
        ledb = 1;
        ledr = 1;
        ledg = 0;       //Groen
        
    }
    else{ 
    motor1PWM = 0;
        
        ledb = 1;       //Rood
        ledr = 0;
        ledg = 1;
    }
    
    
}

int main(){
    pc.baud(115200);
    controller.attach(&motor1_Controller, CONTROLLER_TS);      
    
    while(1){    
    double reference = 10*potMeter1.read();
    double position = RAD_PER_PULSE*Encoder1.getPulses();
    double motor1 = P(reference-position, MOTOR1_KP);
      pc.printf("\r value motor1: %f. reference Pot: %f. Position: %f \n", motor1, reference, position);         
    }
}