Implement proper PID control loops for movement

[yameatmeyourdead]

The current implementation of movement is strictly tied to the controller. With the programming for the IMU and depth detection practically complete, a more rigorous control model must be implemented. Must allow for automatic depth control (e.g. hands of the controller and it keeps current depth) and automatic stabalization to counteract unbalanced forces due to having a load on the manipulator arm. The current vector model provides a good base for expansion.

[yameatmeyourdead]

Implement proper PID control loops for movement

[yameatmeyourdead]

Partially implemented in 2e82d0f
Need to make control signal algorithms, error detection, and setpoint code