Added New Code

Correct Angle finding
Updated the drawing code
- Now displays the digger
- Now has command window in the figure
- Can track either L or I

Introduced new PID code
Added PID structure code
Added new path generation code
Changed max move to reflect new path generation code

Cleaned up the code
Now draws the bucket also

TODO: Add matrix that utilises different gains for when the bucket is in
a different position.