File content as of revision 12:02abb60385c8:

#include "mbed.h"
#include "encoder.h"
#include "HIDScope.h"
#include "MODSERIAL.h" //voor print realterm

#define TSAMP 0.01
#define K_P (0.01)
#define K_I (0  *TSAMP)
#define K_D (0      /TSAMP)
#define I_LIMIT 1.

#define POT_AVG 50

void clamp(float * in, float min, float max);
float pid(float setpoint, float measurement);
AnalogIn potmeter(PTC2);
volatile bool looptimerflag;
float potsamples[POT_AVG];
HIDScope scope(2);

MODSERIAL pc(USBTX,USBRX); //voor print realterm


void setlooptimerflag(void)
{
    looptimerflag = true;
}



int main()
{
    pc.baud(115200); //baudrate instellen voor realterm print
    //start Encoder
    Encoder motor1(PTD0,PTC9);
    // @param int_a Pin to be used as InterruptIn! Be careful, as not all pins on all platforms may be used as InterruptIn.
    // @param int_b second encoder pin, used as DigitalIn. Can be any DigitalIn pin, not necessarily on InterruptIn location
    /*PwmOut to motor driver*/
    PwmOut pwm_motor(PTA5);
    //10kHz PWM frequency
    pwm_motor.period_us(100);
    DigitalOut motordir(PTD1);
    Ticker looptimer;
    looptimer.attach(setlooptimerflag,TSAMP);
    float der, prevpos, stop; 
    der = 0;        //derivative 
    prevpos = 0;    //previous result of motor1.getPosition()
    stop = 0;       //after x amount of whiles while loop stops
    wait(20);
    while(1) {
        float setpoint;
        float new_pwm;        
        while(!looptimerflag);
        looptimerflag = false; //clear flag
        //potmeter value: 0-1
        //setpoint = (potmeter.read()-.5)*500;        
        //new_pwm = (setpoint - motor1.getPosition())*.001; -> P action
        new_pwm = -1; // DIT IS PUUR VOOR DE STEPFUNCTION
        //new_pwm = pid(setpoint, motor1.getPosition());
        clamp(&new_pwm, -1,1);
        //scope.set(0, setpoint);
        scope.set(0, new_pwm);
        scope.set(1, motor1.getPosition());
        // ch 1, 2 and 3 set in pid controller 
        scope.send();
        if(new_pwm > 0)
            motordir = 0; // links 25D motor
        else
            motordir = 1; // rechts 25D motor
        pwm_motor.write(abs(new_pwm));
        
       der = (motor1.getPosition() - prevpos)/TSAMP;
       pc.printf("%f \n", der);
       prevpos = motor1.getPosition();
       stop++;
       if (stop>500) {
       break; 
       }          
    }
}


//clamps value 'in' to min or max when exceeding those values
//if you'd like to understand the statement below take a google for
//'ternary operators'.
void clamp(float * in, float min, float max)
{
    *in > min ? *in < max? : *in = max: *in = min;
}

/*
float pid(float setpoint, float measurement)
{
    float error;
    static float prev_error = 0;
    float           out_p = 0;
    static float    out_i = 0;
    float           out_d = 0;
    error  = setpoint-measurement;
    out_p  = error*K_P;
    out_i += error*K_I;
    out_d  = (error-prev_error)*K_D;
    clamp(&out_i,-I_LIMIT,I_LIMIT);
    prev_error = error;
    scope.set(1,out_p);
    scope.set(2,out_i);
    scope.set(3,out_d);
    return out_p + out_i + out_d;    
}
*/