Industrielle styresystemer - Wiki
Industrielle styresystemer - Wiki
HomingOn : BOOL;
Info |
---|
Type: Function Responsible: Kenneth, Magnus |
Code Block | ||||||
---|---|---|---|---|---|---|
| ||||||
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 |