File content as of revision 4:1a53b06eeb7f:

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

#define K_P .01
//0.007//0.0184997836671646 //0.015
#define K_I 0.00
//.000824387821287097 //0
#define K_D .05
//0.01//.0972091946803081 //0.077

#define POT_AVG 50
void coerce(float * in, float min, float max);
float pid(float setpoint, float measurement);
AnalogIn potmeter(PTC2);
volatile bool looptimerflag;
float potsamples[POT_AVG];

void setlooptimerflag(void)
{
    looptimerflag = true;
}

void potAverager(void)
{

    static uint16_t sample_index = 0;
    float voltage = potmeter.read()-.5;

    potsamples[sample_index] = voltage;

    sample_index++;
    if(sample_index >= POT_AVG)
        sample_index = 0;
}

void bubblesort(float arr[], int size)
{
    int i,j;
    float temp;
    for ( i = 0 ; i <= (size/2)+1 ; i++ )
    {
        for ( j = 0 ; j <= (size/2)+1 - i ; j++ )
        {
            if ( arr[j] > arr[j + 1] )
            {
                temp = arr[j] ;
                arr[j] = arr[j + 1] ;
                arr[j + 1] = temp ;
            }
        }
    }
}

void quicksort(float own_array[], int low, int high)
{
    int i = low;
    int j = high;
    int y = 0;
    /* compare value */
    int z = own_array[(low + high) / 2];
    /* partition */
    do {
        /* find member above ... */
        while(own_array[i] < z) i++;

        /* find element below ... */
        while(own_array[j] > z) j--;

        if(i <= j) {
            /* swap two elements */
            y = own_array[i];
            own_array[i] = own_array[j];
            own_array[j] = y;
            i++;
            j--;
        }
    } while(i <= j);

    /* recurse */
    if(low <= j)
        quicksort(own_array, low, j);

    if(i <= high)
        quicksort(own_array, i, high);
}

float getpotAverage(void)
{
    //float own_array[POT_AVG];
    //memcpy(own_array, potsamples, sizeof(own_array));
    //bubblesort(own_array, POT_AVG-1);
    //return own_array[POT_AVG/2];
    uint16_t valuecounter;
    float sum = 0;
    for(valuecounter = 0 ; valuecounter < POT_AVG ; valuecounter++)
     {
         sum += potsamples[valuecounter];
     }
    return sum / (POT_AVG*1.);
}

int main()
{
    Encoder motor1(PTD0,PTC9);
    Ticker potaverage;
    MODSERIAL pc(USBTX,USBRX);
    PwmOut pwm_motor(PTA12);
    DigitalOut motordir(PTD3);
    potaverage.attach(potAverager,0.002);
    pc.baud(921600);
    Ticker looptimer;
    looptimer.attach(setlooptimerflag,0.01);
    pc.printf("bla");
    while(1) {
        float setpoint;
        float new_pwm;
        while(!looptimerflag);
        looptimerflag = false;
        setpoint = (getpotAverage())*2000;
        //new_pwm = (setpoint - motor1.getPosition())*.001;
        new_pwm = pid(setpoint, motor1.getPosition());
        coerce(&new_pwm, -1,1);
        pc.printf("%f %f %f 0\n", setpoint/2000.+0.5, motor1.getPosition()/2000.+.5,new_pwm/2.+0.5);
        if(new_pwm > 0)
            motordir = 1;
        else
            motordir = 0;
        pwm_motor.write(abs(new_pwm));
    }
}


//coerces 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 coerce(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;
    coerce(&out_i,-0.5,0.5);
    prev_error = error;
    return out_p + out_i + out_d;
}