24_7 Homing

FUNCTION_BLOCK HomingBlokk
VAR_INPUT
	HomingOn : BOOL;
END_VAR
VAR_OUTPUT
END_VAR
VAR
	i : INT := 0;
	curTorque : LREAL := 0;
	homeTorqueThreshold : LREAL := 70;
	homeMoveWeight : LREAL := 0.01;
	
	distances : ARRAY [0..7] OF LREAL;
	motors : ARRAY [0..7] OF AXIS_REF_SM3;
	zeroCalibrated : ARRAY [0..7] OF BOOL;
	maxCalibrated : ARRAY [0..7] OF BOOL;
	
	// Speeds
	lowSpeed : LREAL := 5;
	midSpeed : LREAL := 15;
	highSpeed : LREAL := 30;
END_VAR

motors[0] := Axis1;
motors[1] := Axis2;
motors[2] := Axis3;
motors[3] := Axis4;
motors[4] := Axis5;
motors[5] := Axis6;
motors[6] := Axis7;
motors[7] := Axis8;

IF HomingOn THEN
	FOR i := 0 TO 7 DO
		curTorque := motors[i].fActTorque; 
	
		// First check if the motor is calibrated. It starts as FALSE
		IF NOT(zeroCalibrated[i]) THEN
			IF ABS(curTorque) < homeTorqueThreshold THEN
				globals.motorSpeeds[i] :=   -lowSpeed;
			ELSE
				globals.motorSpeeds[i] := 0;
				zeroCalibrated[i] := TRUE;
				globals.motorOffsetsZero[i] := motors[i].fActPosition;
			END_IF
		END_IF
	END_FOR
END_IF