Untitled

FUNCTION_BLOCK StepMotorControl
VAR_INPUT
	Direction:BOOL;
	Step:BOOL;
END_VAR
VAR_OUTPUT
	Aplus:BOOL;
	Bplus:BOOL;
	Aminus:BOOL;
	Bminus:BOOL;
	
END_VAR
VAR
regulator:Regulator;
stepPos:INT;
prevPos:INT;
state:INT;
END_VAR



IF regulator.CalcPos<0 THEN 
	Direction:=FALSE;
	ELSE
	Direction:=TRUE;
END_IF

CASE stepPos OF 
	0:	//Aplus
		prevPos:=0;
		Aplus:=TRUE;
		Bplus:=FALSE;
		Aminus:=FALSE;
		Bminus:=FALSE;
		
	1: //Bplus
		prevPos:=1;
		Aplus:=FALSE;
		Bplus:=TRUE;
		Aminus:=FALSE;
		Bminus:=FALSE;
	2: //Aminus
		prevPos:=2;
		Aplus:=FALSE;
		Bplus:=FALSE;
		Aminus:=TRUE;
		Bminus:=FALSE;
	3: //Bminus
		prevPos:=3;
		Aplus:=FALSE;
		Bplus:= FALSE;
		Aminus:=FALSE;
		Bminus:= TRUE;
END_CASE

IF regulator.Step THEN 
	IF prevPos=0 THEN 
		IF Direction THEN 
			stepPos:= stepPos+1;
		ELSE
			stepPos:=stepPos-1;
		END_IF
	END_IF
		
	IF prevPos=1 THEN 
		IF Direction THEN 
			stepPos:= stepPos+1;
		ELSE
			stepPos:=stepPos-1;
		END_IF
	END_IF
	
	IF prevPos=2 THEN 
		IF Direction THEN 
			stepPos:=stepPos+1;
		ELSE
			stepPos:=stepPos-1;
		END_IF
	END_IF
		
	IF prevPos=3 THEN 
		IF Direction THEN 
			stepPos:=stepPos+1;
		ELSE
			stepPos:=stepPos-1;
		END_IF
	END_IF
END_IF
prevPos:=stepPos;