FUNCTION_BLOCK "EGL_1_control" { S7_Optimized_Access := 'TRUE' } VERSION : 0.1 VAR_INPUT Force_parameter : DInt; Command : UInt; Position_parameter : DInt; END_VAR VAR_OUTPUT Done : Bool; Error : Bool; END_VAR VAR Desired_Current : Real; Desired_Current_swap : Real; Command_copy : UInt; Desired_Position : Real; Desired_Position_swap : Real; Force_parameter_swap : DInt; END_VAR BEGIN IF NOT(#Command = #Command_copy) THEN #Command_copy := #Command; #Done := FALSE; #Desired_Current_swap := DINT_TO_REAL(DWORD_TO_DINT(SWAP(#Force_parameter))); #Desired_Current := #Desired_Current_swap / 1000; #Desired_Position_swap := DINT_TO_REAL(DWORD_TO_DINT(SWAP(#Position_parameter))); #Desired_Position := #Desired_Position_swap / 1000; CASE #Command_copy OF 0: //reset STATE "EGL_1_Jerk_Activated" := 0; "EGL_1_Acceleration_Active" := 0; "EGL_1_Current_Active" := 0; "EGL_1_Speed_Active" := 0; "EGL_1_Position_Active" := 0; "EGL_1_RelPosition_Active" := 0; "EGL_1_Referencing" := 0; "EGL_1_Restart_Module" := 0; "EGL_1_Acknowledge_Error" := 0; "EGL_1_Stop" := 0; "EGL_1_Quick_Stop" := 1; "EGL_1_Command_Release_A" := 0; "EGL_1_Desired_Position" := 10; "EGL_1_Desired_Speed" := 150; "EGL_1_Desired_Electricity" := 1; "EGL_1_Desired_Acceleration" := 2500; "EGL_1_Desired_Jerk" := 50000; 1: // REFERENCING STATE "EGL_1_Jerk_Activated" := 0; "EGL_1_Acceleration_Active" := 0; "EGL_1_Current_Active" := 0; "EGL_1_Speed_Active" := 0; "EGL_1_Position_Active" := 0; "EGL_1_RelPosition_Active" := 0; "EGL_1_Referencing" := 1; "EGL_1_Restart_Module" := 0; "EGL_1_Acknowledge_Error" := 1; "EGL_1_Stop" := 1; "EGL_1_Quick_Stop" := 1; "EGL_1_Command_Release_A" := 1; "EGL_1_Desired_Position" := 10; "EGL_1_Desired_Speed" := 150; "EGL_1_Desired_Electricity" := 2; "EGL_1_Desired_Acceleration" := 2500; "EGL_1_Desired_Jerk" := 50000; #Error := 0; 2: // GRIPPING STATE IF #Desired_Current = 0 THEN #Desired_Current := 2; // default current ELSE IF #Desired_Current < 0.5 THEN #Desired_Current := 0.5; ELSIF #Desired_Current > 4 THEN #Desired_Current := 4; END_IF; END_IF; "EGL_1_Jerk_Activated" := 0; "EGL_1_Acceleration_Active" := 0; "EGL_1_Current_Active" := 0; "EGL_1_Speed_Active" := 1; "EGL_1_Position_Active" := 0; "EGL_1_RelPosition_Active" := 0; "EGL_1_Referencing" := 0; "EGL_1_Restart_Module" := 0; "EGL_1_Acknowledge_Error" := 0; "EGL_1_Stop" := 1; "EGL_1_Quick_Stop" := 1; "EGL_1_Command_Release_A" := 1; "EGL_1_Desired_Position" := 10; "EGL_1_Desired_Speed" := 80; "EGL_1_Desired_Electricity" := #Desired_Current; "EGL_1_Desired_Acceleration" := 2500; "EGL_1_Desired_Jerk" := 50000; 3: // RELEASE STATE IF #Desired_Position = 0 THEN #Desired_Position := 10; // default current ELSE IF #Desired_Position < 1 THEN #Desired_Position := 1; ELSIF #Desired_Position > 85 THEN #Desired_Position := 85; END_IF; END_IF; "EGL_1_Jerk_Activated" := 0; "EGL_1_Acceleration_Active" := 0; "EGL_1_Current_Active" := 0; "EGL_1_Speed_Active" := 0; "EGL_1_Position_Active" := 1; "EGL_1_RelPosition_Active" := 0; "EGL_1_Referencing" := 0; "EGL_1_Restart_Module" := 0; "EGL_1_Acknowledge_Error" := 0; "EGL_1_Stop" := 1; "EGL_1_Quick_Stop" := 1; "EGL_1_Command_Release_A" := 1; "EGL_1_Desired_Position" := #Desired_Position; "EGL_1_Desired_Speed" := 100; "EGL_1_Desired_Electricity" := 2; "EGL_1_Desired_Acceleration" := 2500; "EGL_1_Desired_Jerk" := 50000; ELSE // Statement section ELSE #Error := 1; END_CASE; END_IF; // Check command progress IF #Command_copy = 1 THEN //referencing IF "EGL_1_Module_Referenced" AND ("EGL_1_Actual_Position" < 0.1 AND "EGL_1_Actual_Position" > -0.1) THEN "EGL_1_Jerk_Activated" := 0; "EGL_1_Acceleration_Active" := 0; "EGL_1_Current_Active" := 0; "EGL_1_Speed_Active" := 0; "EGL_1_Position_Active" := 0; "EGL_1_RelPosition_Active" := 0; "EGL_1_Referencing" := 0; "EGL_1_Restart_Module" := 0; "EGL_1_Acknowledge_Error" := 0; "EGL_1_Stop" := 0; "EGL_1_Quick_Stop" := 1; "EGL_1_Command_Release_A" := 0; "EGL_1_Desired_Position" := 10; "EGL_1_Desired_Speed" := 150; "EGL_1_Desired_Electricity" := 1; "EGL_1_Desired_Acceleration" := 2500; "EGL_1_Desired_Jerk" := 50000; #Done := 1; END_IF; ELSIF #Command_copy = 2 THEN //gripping IF ("EGL_1_Actual_Current" < #Desired_Current + 0.1) AND ("EGL_1_Actual_Current" > #Desired_Current - 0.1) THEN "EGL_1_Jerk_Activated" := 0; "EGL_1_Acceleration_Active" := 0; "EGL_1_Current_Active" := 0; "EGL_1_Speed_Active" := 0; "EGL_1_Position_Active" := 0; "EGL_1_RelPosition_Active" := 0; "EGL_1_Referencing" := 0; "EGL_1_Restart_Module" := 0; "EGL_1_Acknowledge_Error" := 0; "EGL_1_Stop" := 0; "EGL_1_Quick_Stop" := 1; "EGL_1_Command_Release_A" := 0; "EGL_1_Desired_Position" := 10; "EGL_1_Desired_Speed" := 150; "EGL_1_Desired_Electricity" := 1; "EGL_1_Desired_Acceleration" := 2500; "EGL_1_Desired_Jerk" := 50000; #Done := 1; END_IF; ELSIF #Command_copy = 3 THEN //release IF ("EGL_1_Actual_Position" < #Desired_Position + 0.1) AND ("EGL_1_Actual_Position" > #Desired_Position - 0.1) THEN "EGL_1_Jerk_Activated" := 0; "EGL_1_Acceleration_Active" := 0; "EGL_1_Current_Active" := 0; "EGL_1_Speed_Active" := 0; "EGL_1_Position_Active" := 0; "EGL_1_RelPosition_Active" := 0; "EGL_1_Referencing" := 0; "EGL_1_Restart_Module" := 0; "EGL_1_Acknowledge_Error" := 0; "EGL_1_Stop" := 0; "EGL_1_Quick_Stop" := 1; "EGL_1_Command_Release_A" := 0; "EGL_1_Desired_Position" := 10; "EGL_1_Desired_Speed" := 150; "EGL_1_Desired_Electricity" := 1; "EGL_1_Desired_Acceleration" := 2500; "EGL_1_Desired_Jerk" := 50000; #Done := 1; END_IF; ELSE #Done := 1; END_IF; //Check errors IF "EGL_1_Error" THEN #Error := 1; END_IF; END_FUNCTION_BLOCK