TCPControlSubUnit is finished and tested. TrajectoryControllerSubUnit is finished and tested as well.