Line following works now. 
A P and a PD controller were built in to control it. 
The Hough transform works as well now. 
The old method to decide the height is built back in since this new method caused some instability.