Integrate the Mission Logic Planning

[Cruiz102]

Move the important and things that works from the mission planning in the Hydrusv2 repository.

Ideas to keep from previous repository:

• Manage movement though a coordinate system.
• Control System based on very simple error difference.
• Separate the movement to ( Surge, Rotate , Forward) individually
• Make a state machine for going around a point.