FUNCTION_BLOCK FreeMove
VAR_INPUT
freeMoveOn : BOOL;
END_VAR
VAR_OUTPUT
END_VAR
VAR
i : INT := 0;
curTorque : LREAL := 0;
freeMoveTorque : LREAL := 50;
freeMoveWeight : LREAL := 0.1;
motors : ARRAY [0..7] OF AXIS_REF_SM3;
// Speeds
lowSpeed : LREAL := 5;
midSpeed : LREAL := 15;
highSpeed : LREAL := 30;
END_VAR
IF freeMoveOn THEN
motors[0] := Axis1;
motors[1] := Axis2;
motors[2] := Axis3;
motors[3] := Axis4;
motors[4] := Axis5;
motors[5] := Axis6;
motors[6] := Axis7;
motors[7] := Axis8;
FOR i := 0 TO 7 DO
curTorque := motors[i].fActTorque;
IF ABS(curTorque) > freeMoveTorque THEN
globals.motorSpeeds[i] := -curTorque * freeMoveWeight * lowSpeed;
ELSIF ABS(curTorque) > 200 THEN
globals.motorSpeeds[i] := 0;
ELSE
globals.motorSpeeds[i] := 0;
END_IF
END_FOR
END_IF