File content as of revision 1:dc683e88b44e:

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

  const double Pi=3.14;

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

volatile bool looptimerflag;

void setlooptimerflag(void);

double get_radians_from_counts(int counts);

double get_setpoint(double input);

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

        looptimerflag = false;
        
        setpoint = get_setpoint(potmeter.read());
        
        pc.printf("setpoint: %d, position motor %d \n\r", setpoint, motor1.getPulses());
        
        position=motor1.getPulses();
        
        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;
}

// Convert Counts -> Rad       ===> NOG NIET GEBRUIKT
double get_radians_from_counts(int counts)
{
const int gear_ratio =131;
const int ticks_per_magnet_rotation = 32;//X2
const double radian_per_encoder_tick =
2*Pi/(gear_ratio*ticks_per_magnet_rotation);
return counts * radian_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;
}