Robot C code for NXT brick