File content as of revision 0:d3f432c5fb55:

#include "mbed.h"
#include "PID.h"
#include "hcsr04.h"
//#include "HIDScope.h"

//HIDScope    scope(2);

HCSR04 sensor(D2, D3);
float distance = 0;

PwmOut speedPin(D5);
DigitalOut dirPin(D4);

const float dt = 0.001;
PID         controller(-5, 0, 0.00001, dt);
Ticker      controlLoop;
//Ticker      scopeTicker;

void control() {
//    scope.set(0,distance);
    controller.setProcessValue(distance);
    controller.setSetPoint(20);
    double speed = controller.compute();
//    scope.set(1,speed);
    if (speed > 0) {
        dirPin.write(1);
        speedPin.write(speed);
    } else {
        dirPin.write(0);
        speedPin.write(speed * -1);
    }
}

int main()
{
    controller.setInputLimits(5, 50);
    controller.setOutputLimits(-1,1);
    controller.setBias(0);
    controller.setMode(AUTO_MODE);
    
//    scopeTicker.attach_us(&scope, &HIDScope::send, 2e4);
    controlLoop.attach(&control, dt);
    while (true) {
        distance = sensor.distance();
    }
}