Added StepControl(), porting from ControlloMossa, TO complete