Untitled

FUNCTION_BLOCK Regulator 
VAR_INPUT 
	Setpoint:INT;
	Calibrate:BOOL:=TRUE;;
	switch:BOOL;
END_VAR

VAR_OUTPUT
	CalcPos:INT;
	Calibrating:BOOL;
	Step:BOOL;
	Direction:BOOL;
END_VAR
VAR
	i:INT;
	j:INT;
	stepPos:INT;
END_VAR

// j an integer that represents position A+, B+,A- or B- in step position 
// Calibrating mode
IF Calibrate THEN
    Calibrating := TRUE;
    stepPos := i;

    WHILE stepPos > 0 DO
        Step := TRUE;
        Direction := FALSE;
        stepPos := stepPos - 1;
    END_WHILE

    // Check if calibration is complete
    IF stepPos <= 0 THEN
        switch := TRUE;
        switch := FALSE;
        Calibrating := FALSE; // Reset Calibrating mode
		Calibrate:=FALSE;
    END_IF
END_IF

// Back to normal mode
IF Calibrate = FALSE AND switch = FALSE THEN
	Setpoint:=j;
    CalcPos := SetPoint-stepPos;
	Step:=TRUE;
END_IF