Untitled

FUNCTION_BLOCK MotorDriver
VAR_INPUT
	Step:BOOL;
	Direction:BOOL;
END_VAR
VAR_OUTPUT
	Aplus:BOOL;
	Bplus:BOOL;
	Aminus:BOOL;
	Bminus:BOOL;
END_VAR
VAR
	positionState:INT;
	prevpositionState:INT:=0;
	lastStep:BOOL;
END_VAR



IF Step THEN 
	IF Direction THEN 
		CASE positionState OF 
			0: //Aplus
				Aplus:=TRUE;
				Bplus:=FALSE;
				Aminus:=FALSE;
				Bminus:=FALSE;
				positionState:=1;
			1: //Bplus
				Aplus:=FALSE;
				Bplus:=TRUE;
				Aminus:=FALSE;
				Bminus:=FALSE;
				positionState:=2;
			2:	//Aminus
				Aplus:=FALSE;
				Bplus:=FALSE;
				Aminus:=TRUE;
				Bminus:=FALSE;
				positionState:=3;
			3:	//Bminus
				Aplus:=FALSE;
				Bplus:=FALSE;
				Aminus:=FALSE;
				Bminus:=TRUE;
				positionState:=0;
		END_CASE
		ELSE 
			CASE positionState OF 
				0: //Aplus
					Aplus:=TRUE;
					Bplus:=FALSE;
					Aminus:=FALSE;
					Bminus:=FALSE;
					positionState:=3;
				1:	//Bplus
					Aplus:=FALSE;
					Bplus:=TRUE;
					Aminus:=FALSE;
					Bminus:=FALSE;
					positionState:=0;
				2:	//Aminus
					Aplus:=FALSE;
					Bplus:=FALSE;
					Aminus:=TRUE;
					Bminus:=FALSE;
					positionState:=1;
				3:	//Bminus
					Aplus:=FALSE;
					Bplus:=FALSE;
					Aminus:=FALSE;
					Bminus:=TRUE;
					positionState:=2;
			END_CASE
	END_IF
END_IF
lastStep:=Step;