File content as of revision 0:40052f5ca77b:

#include "mbed.h"
//#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
//#include "biquadFilter.h"
#include "encoder.h"

void keep_in_range(double * in, double min, double max);

volatile bool looptimerflag;

void setlooptimerflag(void);
double keep_position_in_range(double * in, double min, double max);

int main() {
    //LOCAL VARIABLES 
    /*Potmeter input*/
    AnalogIn potmeter(A0);
    Encoder motor1(D13,D12,false);
    /* MODSERIAL to get non-blocking Serial*/
    MODSERIAL pc(USBTX,USBRX);
    /* PWM control to motor */
    PwmOut pwm_motor(D5);
    /* Direction pin */
    DigitalOut motordir(D4);
    /* variable to store setpoint in */
    double setpoint;
    /* variable to store pwm value in*/
    double pwm_to_motor;
    /* variable to store position of the motor in */
    double position;
    
    //START OF CODE
    
    pc.printf("bla \n\r");
    
    /*Set the baudrate (use this number in RealTerm too! */
    pc.baud(9600);
    
   /*Create a ticker, and let it call the     */
   /*function 'setlooptimerflag' every 0.01s  */
    Ticker looptimer;
    
    looptimer.attach(setlooptimerflag,0.01);      // calls the looptimer flag every 0.01s
    
    pc.printf("bla \n\r");
    
    motor1.setPosition(0);
    
    //INFINITE LOOP
    
    while(1) {
        /* Wait until looptimer flag is true. */
        while(looptimerflag != true);

        looptimerflag = false;
        
        setpoint = 400;
        
        pc.printf("setpoint: %d, position motor %d \n\r", setpoint, motor1.getPosition());
        
        position=motor1.getPosition();
        
        keep_in_range(&position,-4200,4200);
        
        pc.printf("pwm before keep in range: %d \n\r", position);
        
        /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
        pwm_to_motor = (setpoint - position)*.001;
    

        pc.printf("pwm before keep in range: %d \n\r", pwm_to_motor);
        
        keep_in_range(&pwm_to_motor, -1,1);

        pc.printf("pwm after keep in range: %d \n\r", pwm_to_motor);

        if(pwm_to_motor > 0)
            motordir=1;
        else
            motordir=0; 
        pwm_motor=(abs(pwm_to_motor));
    }
}


// Keep in range function
void keep_in_range(double * in, double min, double max)
{
    *in > min ? *in < max? : *in = max: *in = min;
}

// Looptimerflag function
void setlooptimerflag(void)
{
    looptimerflag = true;
}