File content as of revision 11:a9f4f296d7ed:

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

#define SAMP_TIME 0.01
#define K_P (0.1)
#define K_I (0.03 *SAMP_TIME)
#define K_D (0.0005 /SAMP_TIME)
#define I_LIMIT 1.

#define MAXENCO 300
#define MINENCO 0


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

//define functions
//void slam();
void slam_II();
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;

int main()
{
    while(1)
    {
        cin >> y1;
        cout << y1 << endl;
        //slam();
        slam_II();
    }
}

/*void slam()
{
    float enca;
    enca=encoderA.getPosition();
    
    //if (enca <= 0)
    {
        switch(y1)
        {

            case 1:
                m1_dir=1;
                m1_speed=0.33;
                m1_speedout=m1_speed;
                break;
            case 2:
                m1_dir=1;
                m1_speed=0.66;
                m1_speedout=m1_speed;
                break;
            case 3:
                m1_dir=1;
                m1_speed=1.0;
                m1_speedout=m1_speed;
                break;
            default :
                m1_speed=0;
        }
        while (enca < MAXENCO)
        {
            enca=encoderA.getPosition();
            cout << enca << endl;
        }
        m1_speed=0;
        m1_speedout=m1_speed;
        m1_dir=0;
        cout << "blieb!" << endl;
        m1_speed=0.4;
        m1_speedout=m1_speed;
        while(enca > MINENCO)
        {
            enca=encoderA.getPosition();
            cout << enca << endl;
        }
        m1_speed=0;
        m1_speedout=m1_speed;
    }
}*/

void slam_II()
{
    switch (y1)
    {
        case 1:
            m1_dir=1;
            max_speed=0.33;
            m1_speed.write(0.33);
            break;
        case 2:
            m1_dir=1;
            max_speed=0.66;
            m1_speed.write(0.66);
            break;
        case 3:
            m1_dir=1;
            max_speed=1;
            m1_speed.write(1);
            break;
        default:
            m1_dir=1;
            m1_speed=0;
            break;
    }
    new_speed=pid(MAXENCO,encoderA.getPosition());
    while(new_speed!=0)
    {
        new_speed=pid(MAXENCO,encoderA.getPosition());
        clamp(&new_speed,-max_speed,max_speed);
        if (new_speed>0)
        {
            m1_dir=1;
        }
        else
        {
            m1_dir=0;
        }
        m1_speed.write(fabs(new_speed));
        enca=encoderA.getPosition();
    }
    new_speed=pid(MINENCO,encoderA.getPosition());
    while(new_speed!=0)
    {
        new_speed=pid(MINENCO,encoderA.getPosition());
        clamp(&new_speed,-0.5,0.5);
        if (new_speed>0)
        {
            m1_dir=1;    
        }
        else
        {
            m1_dir=0;
        }
        m1_speed.write(fabs(new_speed));
    }
}

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;
}