File content as of revision 4:9fdad59c03d6:

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

    /* MODSERIAL to get non-blocking Serial*/
    MODSERIAL pc(USBTX,USBRX);

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

volatile bool looptimerflag;

void setlooptimerflag(void);

double get_degrees_from_counts(int counts);

double get_setpoint(double input);

int main() {
    //LOCAL VARIABLES 
    /*Potmeter input*/
    AnalogIn potmeter(A0);
    QEI motor1(D12,D13,NC,32);
    /* 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");
    
    //INFINITE LOOP
    
    while(1) {
        /* Wait until looptimer flag is true. */
        while(looptimerflag != true);

        looptimerflag = false;
        
        // Setpoint calibration
        //setpoint = (potmeter.read()-0.5)*2000;
        setpoint = 15;
        
        // Position calibration
        if ((motor1.getPulses()>4200) || (motor1.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
        {
            motor1.reset();
            pc.printf("RESET \n\r");
        }   
        double conversion_to_degree_counts=0.085877862594198; 
        position = conversion_to_degree_counts * motor1.getPulses();
        
        
                pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", setpoint, motor1.getPulses(), position);
        
        
        // This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor
        // stel setpoint tussen (0 en 360) en position tussen (0 en 360)
        // max verschil: 360 -> dan pwm_to_motor 1 tot aan een verschil van 15 graden-> bij 15 moet pwm_to_motor ong 0.1 zijn 
        // dus     0.1=15*gain      gain=0.0067
        pwm_to_motor = (setpoint - position)*0.0067;    
        
        
        keep_in_range(&pwm_to_motor, -1,1);    // Pass to motor controller but keep it in range!
        pc.printf("pwm %f \n\r", pwm_to_motor);

        if(pwm_to_motor > 0)
            {
            motordir=1;
            pc.printf("if loop pwm_to_motor > 0 \n\r");
            }
        else
            {
            motordir=0;
            pc.printf("else loop pwm_to_motor < 0 \n\r");
            }
        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;
}

// Convert Counts -> Rad       ===> NOG NIET GEBRUIKT
double get_degrees_from_counts(int counts)
{
const int gear_ratio =131;
const int ticks_per_magnet_rotation = 32;//X2 Encoder
const double degrees_per_encoder_tick =
360/(gear_ratio*ticks_per_magnet_rotation);
return counts * degrees_per_encoder_tick;
}

// Get setpoint -> potmeter
double get_setpoint(double input)
{
const float offset = 0.5;
const float gain = 4.0;
return (input-offset)*gain;
}