File content as of revision 0:d566a0281325:

#include "mbed.h"
#include "HIDScope.h"

DigitalOut gpo(D0);
DigitalOut led(LED_RED);
HIDScope scope(2);

const float Kp = 2;
const float Ki = 0.2;
const float Kd = 0;
const float Timestep = 0.001;
float G = 15;          //input
float Output = 0 ; //Starting value
float P = 0 ; // Starting value
float e1 = 0 ; //Starting value 
float e2 = 0 ; // Starting value
float e3;       
float Output_Last;  // Remember previous position
float Y;            // Value that is outputted to motor control
float P_Last = 0;   // Starting position
const float Max_Speed = 400;      //Max speed of the motor
int main()
{
    while (true) {
         P_Last = P;        
         e1 = e2;
         e2 = e3;
         e3 = G - P;
         Output_Last = Output;
         Output = Kp * (e3 - e2) + Output_Last +Ki * e3 + Kd * (e3 - 2*e2 + e1);
             Y = Output;
        if (Output >= 1){
            Y = 1;
        }
        else if (Output <= -1){
            Y = -1;
        }
        P = P_Last + Y * Timestep * Max_Speed;
    wait(0.1f);
    scope.set(0,Output);
    scope.set(1,P);
    scope.send();
    }
    
}