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