Finished PID

Moved moveFor and swingFor to PID, added constants to settings.