File content as of revision 13:76a40e4c8982:

#include "mbed.h"
#include "encoder.h"
#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);
//-------------------------------------------------------------------------------------------Input potentiometer-------------------------------
AnalogIn potmeter(PTC2);
volatile bool looptimerflag;
float potsamples[POT_AVG];


void setlooptimerflag(void)
{
    looptimerflag = true;
}

int main()
{
    Encoder motor1(PTD0,PTC9);
    PwmOut pwm_motor(PTA5);
    pwm_motor.period_us(100);
    DigitalOut motordir(PTD1);
    Ticker looptimer;
    looptimer.attach(setlooptimerflag,TSAMP);
    while(1) {
        float setpoint;
        float new_pwm;
        while(!looptimerflag);
        looptimerflag = false;
        //---------------------------------------------------------leest potentiometer (?), probably won't do what I want it to do ------------------
        //---------------------------------------------------------int 6? ---------------------------------------------------------------------------
        if ( 4.5 < potmeter.read() && potmeter.read()< 6.0 && angle<maxangle)
        {
            setpoint = v3; //experimenteel bepaald, snelheid om bovenste goal te raken
        }
        else if ( 3 < potmeter.read())
        {
            setpoint = v2; //experimenteel bepaald, snelheid om middelste goal te raken
        }
        else if (1.5 < potmeter.read())
        {
            setpoint = v1; //experimenteel bepaald, snelheid om bovenste goal te raken
        }
        else 
        {
            setpoint = 0;
        }
        // setpoint = (potmeter.read()-.5)*500;  
        //----------------------------------------------------------------------new_pwm = getal?.....----------------------------------------------------
        new_pwm = pid(setpoint, motor1.getPosition());
        clamp(&new_pwm, -1,1);
        //-------------------------------------------------------------------------------------output motor richting-------------------------------------
        if(new_pwm > 0)
            motordir = 0;
        else
            motordir = 1;
        //-------------------------------------------------------------------------------output motorsnelhied (wij willen 4 distinctive stappen)---------
        pwm_motor.write(abs(new_pwm));
    }
}

void clamp(float * in, float min, float max)
{
    *in > min ? *in < max? : *in = max: *in = min;
}


float pid(float setpoint, float measurement)
{
    //What I want: if getposition gives value beneath min v=0!, If above max: return to 0 position(from the way it came)
    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;
    return out_p + out_i + out_d;
}