Code for FIRA robotics competition