Added timebase to FSM, added heartbeat state machine, plus general writeData function in BioloidController