24_7 FreeMove

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