Fixed broken PID code. Replaced by a different algorithm.

Also did some raw tuning of the PID according to the new algorithm