24_2 Funksjonsblokk Servomotor

FUNCTION_BLOCK FbServoMotor
VAR_INPUT
	posServo1									: LREAL;
	posServo2									: LREAL;
	servo1Status								: BOOL;
	servo2Status								: BOOL;
	execute										: BOOL;
END_VAR
VAR_OUTPUT
	servo1PosReached							: BOOL;
	servo2PosReached							: BOOL;
END_VAR
VAR
	moveServo1									: MC_MoveAbsolute;
	moveServo2									: MC_MoveAbsolute;
END_VAR

IF  Servo1Status AND  Servo2Status THEN
	MoveServo1(Axis:=Servo1, Execute:= execute, Position:= posServo1, Velocity:= 100, Acceleration:= 100, Deceleration:= 2000);
	MoveServo2(Axis:=Servo2, Execute:= execute, Position:= posServo2, Velocity:= 100, Acceleration:= 100, Deceleration:= 2000);
END_IF
IF MoveServo1.Done THEN
	MoveServo1(Axis:=Servo1, Execute:= FALSE, Position:= posServo1, Velocity:= 100, Acceleration:= 100, Deceleration:= 2000);
END_IF
IF MoveServo2.Done THEN
	MoveServo2(Axis:=Servo2, Execute:= FALSE, Position:= posServo2, Velocity:= 100, Acceleration:= 100, Deceleration:= 2000);
END_IF

servo1PosReached:= MoveServo1.Done;
servo2PosReached:= MoveServo2.Done;