New control scheme pretty much finished, needs some cleaning