File content as of revision 7:ddd7fb357786:

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

MODSERIAL pc(USBTX,USBRX);
DigitalIn button(PTA4); // PTA4 is used as a button controller (0 when pressed and 1 when not pressed)

volatile bool looptimerflag;
const double cw=0;          // zero is clockwise (front view)
const double ccw=1;         // one is counterclockwise (front view)

// Functions used (described after main)
void keep_in_range(double * in, double min, double max);
void setlooptimerflag(void);
double get_degrees_from_counts(int counts);
double get_setpoint(double input);

// MAIN function
int main() {
    AnalogIn potmeter(A0);
    QEI motor1(D12,D13,NC,32);
    PwmOut pwm_motor(D5);
    DigitalOut motordir(D4);
    double setpoint;
    double pwm_to_motor;
    double position;
    
    //START OF CODE 
    pc.printf("Start of code \n\r");
    
    pc.baud(9600);          // Set the baudrate
    
    // Tickers 
    Ticker looptimer;                             // Ticker called looptimer to set a looptimerflag
    looptimer.attach(setlooptimerflag,0.01);      // calls the looptimer flag every 0.01s
    
    pc.printf("Start infinite loop \n\r");
    wait (3);
    
    //INFINITE LOOP
    
    while(1) 
        {                                                                           // Start while loop
         if (button.read() < 0.5){                   //if button pressed
             motordir = cw;                          // zero is clockwise (front view)
             pwm_motor = 0.5f;                       // motorspeed
            
            pc.printf("positie = %d \r\n", motor1.getPulses()); }
        else
        {
        /* 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");
        }   
             
// gear ratio motor = 131
// ticks per magnet rotation = 32 (X2 Encoder)
// One revolution = 360 degrees
// degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198 

        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 + e_prev;    
        e_prev=(setpoint - position)
        
        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=ccw;
            pc.printf("if loop pwm_to_motor > 0 \n\r");
            }
        else
            {
            motordir=cw;
            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;
}

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