diff --git a/BaseComponents.tsproj b/BaseComponents.tsproj index 4b3d69d..1cb7207 100644 --- a/BaseComponents.tsproj +++ b/BaseComponents.tsproj @@ -136,7 +136,7 @@ - + BasicComponents Instance {08500001-0000-0000-F000-000000000064} diff --git a/BasicComponents/BasicComponents.plcproj b/BasicComponents/BasicComponents.plcproj index 2bf2adc..010b0ee 100644 --- a/BasicComponents/BasicComponents.plcproj +++ b/BasicComponents/BasicComponents.plcproj @@ -79,6 +79,12 @@ Code + + Code + + + Code + Code @@ -302,8 +308,8 @@ - - + + "<ProjectRoot>" {192FAD59-8248-4824-A8DE-9177C94C195A} @@ -2666,6 +2672,12 @@ 410,5410 + {eeeeeeee-3909-4298-8022-501ac3238667} + + "{eeeeeeee-3909-4298-8022-501ac3238667}" + + + {F66C7017-BDD8-4114-926C-81D6D687E35F} "{F66C7017-BDD8-4114-926C-81D6D687E35F}" @@ -2678,16 +2690,16 @@ - - - System.Boolean - System.Collections.Hashtable - System.Int32 - {54dd0eac-a6d8-46f2-8c27-2f43c7e49861} - System.String - System.UInt32 - - + + + System.Boolean + System.Collections.Hashtable + System.Int32 + {54dd0eac-a6d8-46f2-8c27-2f43c7e49861} + System.String + System.UInt32 + + \ No newline at end of file diff --git a/BasicComponents/POUs/Components/AxisPTP/E_AXIS_PTP_STATE.TcDUT b/BasicComponents/POUs/Components/AxisPTP/E_AXIS_PTP_STATE.TcDUT index 8c6fe63..40c18a8 100644 --- a/BasicComponents/POUs/Components/AxisPTP/E_AXIS_PTP_STATE.TcDUT +++ b/BasicComponents/POUs/Components/AxisPTP/E_AXIS_PTP_STATE.TcDUT @@ -13,6 +13,7 @@ TYPE E_AXIS_PTP_STATE : MOVING_ABSOLUTE, MOVING_RELATIVE, MOVING_VELOCITY, + MOVING_MODULO, HALTING, WAIT_FOR_DISABLE, ERROR, diff --git a/BasicComponents/POUs/Components/AxisPTP/FB_AxisPTP.TcPOU b/BasicComponents/POUs/Components/AxisPTP/FB_AxisPTP.TcPOU index 238d5dd..7fa3bf0 100644 --- a/BasicComponents/POUs/Components/AxisPTP/FB_AxisPTP.TcPOU +++ b/BasicComponents/POUs/Components/AxisPTP/FB_AxisPTP.TcPOU @@ -7,7 +7,7 @@ VAR_INPUT xCalibrationCam AT %I* : BOOL; xInvertCalibrationCam : BOOL := FALSE; xEnablePositive : BOOL := TRUE; - xEnableNegative : BOOL := TRUE;; + xEnableNegative : BOOL := TRUE; rOverride : REAL := 100.0; lrVelocity : LREAL; @@ -23,6 +23,7 @@ VAR_OUTPUT lrActPosition : LREAL; xIsStopped : BOOL; xBusy : BOOL; + xDone : BOOL; xError : BOOL; END_VAR VAR @@ -36,6 +37,7 @@ VAR _fbMoveAbsolute : MC_MoveAbsolute; _fbMoveRelative : MC_MoveRelative; _fbMoveVelocity : MC_MoveVelocity; + _fbMoveModulo : MC_MoveModulo; _fbHalt : MC_Halt; _fbReset : MC_Reset; @@ -49,6 +51,7 @@ VAR _xStartMoveAbsolute : BOOL; _xStartMoveRelative : BOOL; _xStartMoveVelocity : BOOL; + _xStartMoveModulo : BOOL; _xHalt : BOOL; _xReset : BOOL; @@ -61,6 +64,7 @@ VAR _xExecuteMoveAbs : BOOL; _xExecuteMoveRel : BOOL; _xExecuteMoveVelocity : BOOL; + _xExecuteMoveModulo : BOOL; _xExecuteHalt : BOOL; @@ -74,7 +78,9 @@ VAR _eHomingMode : MC_HomingMode := MC_HomingMode.MC_DefaultHoming; _xCalibrationCam : BOOL; - _eMoveVelDirection : MC_Direction; + _eMoveDirection : MC_Direction; + + _xCanExecNewCmd : BOOL; _eState : E_AXIS_PTP_STATE; @@ -83,6 +89,7 @@ VAR // Ouput buffers // ============= + _xDone : BOOL; _xBusy : BOOL; _xError : BOOL; END_VAR @@ -91,6 +98,10 @@ END_VAR @@ -403,11 +475,11 @@ END_IF]]> - - + + + + + + - - - 0 THEN +IF rTn > 0 THEN _rDeltaIntegral := (rKp * _rT / rTn) * _rError; ELSE _rDeltaIntegral := 0; @@ -74,9 +69,12 @@ END_IF _rIntegral := _rIntegral + _rDeltaIntegral; // Reset integral with deactivated integral time -IF (rTn = 0.0) AND (_rIntegral <> 0) THEN +IF (rTn <= 0.0) THEN _rIntegral := 0.0; -END_IF]]> +END_IF + +// Calculate controller output +rMV := _rProportional + _rIntegral;]]> \ No newline at end of file diff --git a/BasicComponents/POUs/Components/Controller/FB_PID.TcPOU b/BasicComponents/POUs/Components/Controller/FB_PID.TcPOU new file mode 100644 index 0000000..beb1782 --- /dev/null +++ b/BasicComponents/POUs/Components/Controller/FB_PID.TcPOU @@ -0,0 +1,108 @@ + + + + + + 0 THEN + _rDeltaIntegral := (rKp * _rT / rTn) * _rError; +ELSE + _rDeltaIntegral := 0; +END_IF + +// Only add new integral part if we are going away from the upper or lower bound +IF (xSaturatedUpper AND (_rDeltaIntegral > 0.0)) THEN + _rDeltaIntegral := 0.0; +END_IF + +IF (xSaturatedLower AND (_rDeltaIntegral < 0.0)) THEN + _rDeltaIntegral := 0.0; +END_IF + +// Calculate integral part +_rIntegral := _rIntegral + _rDeltaIntegral; + +// Reset integral with deactivated integral time +IF (rTn <= 0.0) THEN + _rIntegral := 0.0; +END_IF + +IF rTv > 0.0 THEN + _rDeltaPV := (rPV - _rPVLast) / _rT; + + // Unfiltered derivative + _rDRaw := -rKp * rTv * _rDeltaPV; + + // Filter with PT1 + IF rTdFilter > 0.0 THEN + _rDerivative := _rDerivative + (_rT / (rTdFilter + _rT)) * (_rDRaw - _rDerivative); + ELSE + _rDerivative := _rDRaw; + END_IF +ELSE + _rDerivative := 0.0; +END_IF + +_rPVLast := rPV; + +// Calculate controller output +rMV := _rProportional + _rIntegral + _rDerivative;]]> + + + \ No newline at end of file diff --git a/BasicComponents/POUs/Components/Controller/FB_RampGen.TcPOU b/BasicComponents/POUs/Components/Controller/FB_RampGen.TcPOU new file mode 100644 index 0000000..95c302d --- /dev/null +++ b/BasicComponents/POUs/Components/Controller/FB_RampGen.TcPOU @@ -0,0 +1,86 @@ + + + + + + 0.0); + RETURN; +END_IF + + +_rDiff := rTarget - rOut; + + +IF _rDiff > 0 THEN + // Aufwärtsrampe + _rStep := rRiseRate * _rT; + + IF _rStep > _rDiff THEN + rOut := rTarget; + ELSE + rOut := rOut + _rStep; + END_IF + +ELSIF _rDiff < 0 THEN + // Abwärtsrampe + _rStep := rFallRate * _rT; + + IF _rStep > ABS(_rDiff) THEN + rOut := rTarget; + ELSE + rOut := rOut - _rStep; + END_IF +END_IF + + +xBusy := (ABS(rTarget - rOut) > 0.0);]]> + + + \ No newline at end of file