Code for the UF Gator Submerisbles ROV team