Algorithm for control