C code for Autonomous Robot Systems 31385