finished controller. need to tune gains