Updated controller and algorithm