Arduino code for RoboCup rescue