File content as of revision 14:e9ab81429271:

#include "mbed.h"
#include "HIDScope.h"
//#include "mbed-dsp.h"
#include "encoder.h"
//#include <iostream>
#include "MODSERIAL.h"

#define SAMP_TIME 0.01
#define K_P (0.001)
#define K_I (0.000001 *SAMP_TIME)
#define K_D (0.000005 /SAMP_TIME)
#define I_LIMIT 1.

#define MAXENCO 320
#define MINENCO 0


//define in and output
MODSERIAL pc(USBTX,USBRX);
Encoder encoderA(PTD0,PTD2);
PwmOut m1_speed(PTA5);
DigitalOut m1_dir(PTA4);

//define functions
void slam(int emg_value);
void clamp(float * in, float min, float max);
float pid(float rev_value, float mea_value);

//define global variables
//int y1;
float new_speed;
float max_speed;
float enca;
volatile bool looptimerflag;

void setlooptimerflag(void)
{
    looptimerflag = true;
}

int main()
{
    
    int y1;
    Ticker looptimer;
    pc.baud(9600);
    looptimer.attach(setlooptimerflag,SAMP_TIME);
    while(1)
    {   
        if(! pc.readable())
        {   
            pc.printf("Main %d\n\r", encoderA.getPosition());
            wait(0.1);
        }
        else
        {    
            y1 = pc.getc()-48;
            pc.printf("%d\n\r", y1 );
            slam(y1);
        }
    }
}

void slam(int emg_value)
{
    //pc.printf( "Ik begin met de slafunctie\n\r");
    //pc.printf( encoderA.getPosition() << endl << "check\n\r");
    pc.printf("Sla\n\r%d\n\r", encoderA.getPosition());
    switch (emg_value)
    {
        case 1:
            pc.printf( "zaak1\n\r");
            m1_dir=1;
            max_speed=0.33;
            //m1_speed.write(0.33);
            //pc.printf( "Motor 1 draait\n\r");
            break;
        case 2:
            pc.printf( "zaak2\n\r");
            m1_dir=1;
            max_speed=0.66;
            //m1_speed.write(0.66);
            pc.printf( "Motor 1 draait\n\r");
            break;
        case 3:
            pc.printf( "zaak3\n\r");
            m1_dir=1;
            max_speed=1;
            //m1_speed.write(1);
            pc.printf( "Motor 1 draait\n\r");
            break;
        default:
            m1_dir=1;
            m1_speed=0;
            break;
    }
    //pc.printf( "Ik ben uit de switch\n\r");
    while(!looptimerflag);
    looptimerflag=false;
    new_speed=pid(MAXENCO,encoderA.getPosition());
    clamp(&new_speed,-max_speed,max_speed);
    if (new_speed>0)
    {
        m1_dir=1;
    }
    else if (new_speed<0)
    {
        m1_dir=0;
    }
    m1_speed.write(fabs(new_speed));

    while(new_speed<-0.1 || new_speed>0.1)
    {
        while(!looptimerflag);
        looptimerflag=false;
        //pc.printf( "Eat, Sleep, Rave, Repeat\n\r");
        new_speed=pid(MAXENCO,encoderA.getPosition());
        clamp(&new_speed,-max_speed,max_speed);
        if (new_speed>0)
        {
            m1_dir=1;
        }
        else if (new_speed<0)
        {
            m1_dir=0;
        }
        m1_speed.write(fabs(new_speed));
        //pc.printf( new_speed << endl;
    }
    pc.printf( "Ik ben uit de eerste whilelus\n\r");
    //pc.printf( encoderA.getPosition() << endl << "check\n\r");
    pc.printf("%d\n\r", encoderA.getPosition());
    while(!looptimerflag);
    looptimerflag=false;
    new_speed=pid(MINENCO,encoderA.getPosition());
    clamp(&new_speed,-max_speed,max_speed);
    if (new_speed>0)
    {
        m1_dir=1;
    }
    else if (new_speed<0)
    {
        m1_dir=0;
    }
    m1_speed.write(fabs(new_speed));
    
    while(new_speed<-0.1 || new_speed>0.1)
    {
        while(!looptimerflag);
        looptimerflag=false;
        new_speed=pid(MINENCO,encoderA.getPosition());
        clamp(&new_speed,-0.5,0.5);
        if (new_speed>0)
        {
            m1_dir=1;    
        }
        else if (new_speed<0)
        {
            m1_dir=0;
        }
        m1_speed.write(fabs(new_speed));
    }
    m1_speed.write(0);
    pc.printf( "Ik ben uit de tweede whilelus\n\r");
    //pc.printf( encoderA.getPosition() << endl << "check\n\r");
    pc.printf("%d\n\r", encoderA.getPosition());
}

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

float pid(float rev_value, float mea_value)
{
    float error;
    static float prev_error = 0;
    float p_out = 0;
    static float i_out = 0;
    float d_out = 0;
    error = rev_value - mea_value;
    p_out = error * K_P;
    i_out += error * K_I;
    d_out = (error - prev_error) * K_D;
    clamp(&i_out,-I_LIMIT,I_LIMIT);
    prev_error=error;
    return p_out + i_out + d_out;
}