Code for the CPP Robosub Vehicle