File content as of revision 28:fa85793c50bb:

#include "mbed.h"
#include "encoder.h"

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

#define TSAMP 0.01
#define WACHTEN 1
#define SLAAN 2
#define TERUGKEREN 3
#define ANGLEMAX 1000
#define ANGLEMIN 0

float pid(float setspeed, float measurement);
float new_pwm;
DigitalOut motordir2(PTA4);
PwmOut pwm_motor2(PTA5);
Encoder motor2(PTD0,PTD2);

volatile bool looptimerflag;

void setlooptimerflag(void)
{
    looptimerflag = true;
}

int main ()
{
    pwm_motor2.write(0.3); 
    motordir2 = 0;
    
    int toestand = TERUGKEREN; //initial condition
    int EMG = 1;
    float setspeed = 0, V3=40, V2=30, V1 =20, Vreturn= 15;//V in counts/s
    
    Ticker looptimer;
    looptimer.attach(setlooptimerflag,TSAMP);
    while(1) 
    {
        while(!looptimerflag);
        looptimerflag = false;
        
        if (motor2.getPosition()<= ANGLEMIN && toestand != SLAAN)
        {
            toestand = WACHTEN;
        }
        
        if (EMG != 0 && toestand == WACHTEN) // iets vinden om te testen... ipv 1
        {
            toestand = SLAAN;
            if ( EMG==3)
            {
                setspeed = V3;
            }
             if ( EMG==2)
            {
                setspeed = V2;
            }
             if ( EMG==1)
            {
               setspeed = V1;
            }
        }
        
        if (toestand == SLAAN && motor2.getPosition() >= ANGLEMAX)
        {
            toestand = TERUGKEREN;
        }        
        if (toestand == TERUGKEREN)
        {
            new_pwm = pid(Vreturn, motor2.getSpeed());
            pwm_motor2.write(new_pwm);  
            motordir2 = 1;
        }
         if (toestand == WACHTEN)
        {
            pwm_motor2.write(0); 
        }
        if (toestand == SLAAN)
        {
            new_pwm = pid(setspeed, motor2.getSpeed());
            motordir2 = 0; 
            pwm_motor2.write(new_pwm);
        }
            
    }//end while
}//end main


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

float pid(float setspeed, float measurement)
{
    float error;
    static float prev_error = 0;
    float           out_p = 0;
    static float    out_i = 0;
    float           out_d = 0;
    error  = setspeed-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;
}