Add calculation for height control. (Method names/variables probably needs to be renamed so its easier to understand)