final mbed code, close enough