State machine test

Dependencies:   C12832 RangeFinder mbed-rtos mbed