Control law for steering/collision avoidance

[alexispascual]

Create library for control law

Methods:

• Get current angle of traverse
• Get theta dot, given current angle of traverse and two distances
• Get pulse width modification for right wheel
• Get pulse width modification for left wheel

[alexispascual]

Added code for steering in a straight path. Architecture is not pretty but I think it will work. Will test tomorrow.

[alexispascual]

Architecture is now prettier and works well. Will now have to calibrate k_d, k_theta, and turn durations

[alexispascual]

Didn't work. Wheels have too high of a friction with the floor to do fine corrections. Modified the algorithm to be more crude but workable.