Failed PIDbot implementation attempt. Problems with angle value wrap-around. Simple math escaping me.
Added helper functions for the VPS and code to initialize the VPS on startup.
Gave math functions their own file.