File content as of revision 0:1d02168661ec:

/***************************************/
/*                                     */
/*   BRONCODE GROEP 5, MODULE 9, 2014  */
/*       *****-THE SLAP-******         */
/*                                     */
/* -Dominique Clevers                  */
/* -Rianne van Dommelen                */
/* -Daan de Muinck Keizer              */
/* -David den Houting                  */
/* -Marjolein Thijssen                 */
/***************************************/
#include "mbed.h"
#include "encoder.h"
#include "MODSERIAL.h"
#include "HIDscope.h"

//POSITIE EN SNELHEID UITLEZEN OP PC
int main()
{
while(1) //Loop
    {
    /** Make encoder object.
    * First pin should be on PTAx or PTDx because those pins can be used as InterruptIn
    * Second pin can be any digital input
    */
    Encoder motor1(PTD0,PTC9);
    /*Use USB serial to send values*/
    MODSERIAL pc(USBTX,USBRX);
    /*Set baud rate to 115200*/
    pc.baud(115200);
    while(1) { //Loop
        /**Wait to prevent buffer overflow, and to keep terminal readable (not scrolling too fast)*/
        wait(0.2);
        /** print position (integer) and speed (float) to the PC*/
        pc.printf("pos: %d, speed %f \r\n",motor1.getPosition(), motor1.getSpeed());
    }
}

//AANSTUREN MOTOR (POSITIE EN SNELHEID)
#define TSAMP 0.01
#define K_P (0.1)
#define K_I (0.03  *TSAMP)
#define K_D (0.001 /TSAMP)
#define I_LIMIT 1.

#define M1_PWM PTC8
#define M1_DIR PTC9
#define M2_PWM PTA5
#define M2_DIR PTA4

//#define POT_AVG 50

void clamp(float * in, float min, float max);
float pid(float setpoint, float measurement);
volatile bool looptimerflag;
//float potsamples[POT_AVG];
HIDScope scope(6);


void setlooptimerflag(void)
{
    looptimerflag = true;
}

int main()
{
    AnalogIn potmeter(PTC2);
    //start Encoder-> first pin should be PTDx or PTAx, second pin doesn't matter
    Encoder motor1(PTD0,PTA13);
    /*PwmOut to motor driver*/
    PwmOut pwm_motor(M2_PWM);
    //10kHz PWM frequency
    pwm_motor.period_us(75);
    DigitalOut motordir(M2_DIR);
    Ticker looptimer;
    looptimer.attach(setlooptimerflag,TSAMP);
    while(1) {
        int16_t setpoint;
        float new_pwm;
        /*wait until timer has elapsed*/
        while(!looptimerflag);
        looptimerflag = false; //clear flag
        /*potmeter value: 0-1*/
        setpoint = (potmeter.read()-.5)*500;        
        /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
        new_pwm = pid(setpoint, motor1.getPosition());
        clamp(&new_pwm, -1,1);
        scope.set(0, setpoint);
        scope.set(4, new_pwm);
        scope.set(5, motor1.getPosition());
        // ch 1, 2 and 3 set in pid controller */
        scope.send();
        if(new_pwm < 0)
            motordir = 0;
        else
            motordir = 1;
        pwm_motor.write(abs(new_pwm));
    }
}


//clamps 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 clamp(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;
    clamp(&out_i,-I_LIMIT,I_LIMIT);
    prev_error = error;
    scope.set(1,out_p);
    scope.set(2,out_i);
    scope.set(3,out_d);
    return out_p + out_i + out_d;
}