Skip to content
This repository was archived by the owner on Mar 25, 2025. It is now read-only.

Commit fc58c17

Browse files
rruiter87Roald Ruiter
rruiter87
and
Roald Ruiter
authored
add SetForceTask (#720)
Co-authored-by: Roald Ruiter <[email protected]>
1 parent 555a989 commit fc58c17

File tree

2 files changed

+210
-0
lines changed

2 files changed

+210
-0
lines changed

Diff for: ‎src/TcoDrivesBeckhoff/src/XaeTcoDrivesBeckhoff/TcoDrivesBeckhoff/POUs/Prototypes/TcoDriveSimple/MotionTasks/SetForceTask.TcPOU

+207
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,207 @@
1+
<?xml version="1.0" encoding="utf-8"?>
2+
<TcPlcObject Version="1.1.0.1" ProductVersion="3.1.4024.12">
3+
<POU Name="SetForceTask" Id="{202acf02-6d0d-4bca-9f77-4501493a923b}" SpecialFunc="None">
4+
<Declaration><![CDATA[FUNCTION_BLOCK SetForceTask EXTENDS TcoTask IMPLEMENTS ITcoDriveSimple
5+
VAR
6+
{attribute addProperty Name "<#Force#>"}
7+
_force_percent : LREAL;
8+
{attribute addProperty Name "<#Velocity limit#>"}
9+
_velocityLimit : LREAL;
10+
{attribute addProperty Name "<#Torque ramp#>"}
11+
_torqueRamp : LREAL;
12+
END_VAR
13+
VAR
14+
{attribute 'hide'}
15+
_axisRef : REFERENCE TO TcoAxisRef;
16+
{attribute 'hide'}
17+
_mcForceControl : Tc2_MC2.MC_TorqueControl;
18+
{attribute 'hide'}
19+
_forceOptions : Tc2_MC2.ST_TorqueControlOptions;
20+
{attribute 'hide'}
21+
_message : STRING(125);
22+
END_VAR
23+
]]></Declaration>
24+
<Implementation>
25+
<ST><![CDATA[IF NOT __ISVALIDREF(_axisRef) THEN
26+
IF __ISVALIDREF(DriveRef) THEN
27+
_axisRef REF= DriveRef.AxisRef;
28+
ELSE
29+
RETURN;
30+
END_IF
31+
END_IF
32+
33+
IF Execute() THEN
34+
_forceOptions.EnableManualTorqueStartValue := TRUE;
35+
_forceOptions.ManualTorqueStartValue := _axisRef.NcToPlc.ActTorque;
36+
_mcForceControl(
37+
Axis:=_axisRef,
38+
Execute:=NOT _mcForceControl.Busy,
39+
Torque:=_force_percent,
40+
BufferMode:=Tc2_MC2.MC_Aborting,
41+
ContinuousUpdate:=TRUE,
42+
Options:=_forceOptions,
43+
TorqueRamp:=_torqueRamp,
44+
VelocityLimitHigh:=_velocityLimit,
45+
VelocityLimitLow:=_velocityLimit * -1,
46+
Busy=>,
47+
Active=>,
48+
CommandAborted=>,
49+
Error=>,
50+
ErrorID=>
51+
);
52+
53+
IF _mcForceControl.CommandAborted THEN
54+
THIS^.Abort();
55+
END_IF
56+
57+
DoneWhen(_mcForceControl.InTorque);
58+
ThrowWhen(_mcForceControl.Error);
59+
END_IF
60+
]]></ST>
61+
</Implementation>
62+
<Property Name="DriveRef" Id="{3d05b5df-cf94-4ecb-a8c7-154657b19d59}">
63+
<Declaration><![CDATA[PROPERTY DriveRef : REFERENCE TO TcoDriveSimple
64+
]]></Declaration>
65+
<Get Name="Get" Id="{5be2c727-f826-4b2c-b293-d8b1aed20eb9}">
66+
<Declaration><![CDATA[VAR
67+
_drive : ITcoDriveSimple := 0;
68+
END_VAR
69+
]]></Declaration>
70+
<Implementation>
71+
<ST><![CDATA[IF __QUERYINTERFACE(THIS^.Parent, _drive) THEN
72+
DriveRef REF= _drive.DriveRef;
73+
END_IF
74+
]]></ST>
75+
</Implementation>
76+
</Get>
77+
</Property>
78+
<Method Name="InvokeWithParams" Id="{96f3ce16-2674-4918-9288-6d9cb1447a5e}">
79+
<Declaration><![CDATA[METHOD INTERNAL InvokeWithParams : ITcoTaskStatus
80+
VAR_INPUT
81+
inForce_percent : LREAL;
82+
inVelocityLimit : LREAL;
83+
inTorqueRamp : LREAL;
84+
END_VAR
85+
]]></Declaration>
86+
<Implementation>
87+
<ST><![CDATA[_force_percent := inForce_percent;
88+
_velocityLimit := inVelocityLimit;
89+
_torqueRamp := inTorqueRamp;
90+
InvokeWithParams := THIS^.Invoke();
91+
]]></ST>
92+
</Implementation>
93+
</Method>
94+
<Method Name="OnAbort" Id="{7b302d13-93b1-428d-b6d4-ce1dbc732d0e}">
95+
<Declaration><![CDATA[(*~
96+
<docu>
97+
<summary>
98+
Executes when task is aborted.
99+
</summary>
100+
</docu>
101+
~*)
102+
METHOD PROTECTED OnAbort
103+
]]></Declaration>
104+
<Implementation>
105+
<ST><![CDATA[IF NOT __ISVALIDREF(_axisRef) THEN
106+
IF __ISVALIDREF(DriveRef) THEN
107+
_axisRef REF= DriveRef.AxisRef;
108+
ELSE
109+
RETURN;
110+
END_IF
111+
END_IF
112+
113+
THIS^.DriveRef.Stop(-1, -1);
114+
_mcForceControl(Axis:=_axisRef, Execute:=FALSE);
115+
THIS^.DriveRef._movementState.Restore();
116+
]]></ST>
117+
</Implementation>
118+
</Method>
119+
<Method Name="OnDone" Id="{9007e457-9563-4c42-bf13-1f19f60b8de2}">
120+
<Declaration><![CDATA[(*~
121+
<docu>
122+
<summary>
123+
Executes when task reaches the ```Done``` state .
124+
</summary>
125+
</docu>
126+
~*)
127+
METHOD PROTECTED OnDone
128+
]]></Declaration>
129+
<Implementation>
130+
<ST><![CDATA[_mcForceControl(Axis:=_axisRef, Execute:=FALSE);
131+
THIS^.DriveRef._movementState.Restore();
132+
]]></ST>
133+
</Implementation>
134+
</Method>
135+
<Method Name="OnError" Id="{4e24a163-b359-47b6-a0a7-57e7d9a45c78}">
136+
<Declaration><![CDATA[(*~
137+
<docu>
138+
<summary>
139+
Executes when task reaches the ```Error``` state.
140+
<para>
141+
Do not confuse with <see cref="PlcDocu.TcoCore.TcoTask.WhileError()"/>, that is called cyclically until '''Error''' state is left.
142+
</para>
143+
</summary>
144+
</docu>
145+
~*)
146+
METHOD PROTECTED OnError
147+
]]></Declaration>
148+
<Implementation>
149+
<ST><![CDATA[THIS^.DriveRef._movementState.Restore();
150+
151+
IF NOT _axisRef.Status.Error THEN
152+
DriveRef.SetErrorID(DriveRef._axisStatus.ErrorId);
153+
END_IF
154+
155+
_mcForceControl(Axis:=_axisRef, Execute:=FALSE);
156+
]]></ST>
157+
</Implementation>
158+
</Method>
159+
<Method Name="OnStart" Id="{be299f11-c5f4-4588-8158-9ce4a7891dec}">
160+
<Declaration><![CDATA[METHOD PROTECTED OnStart
161+
]]></Declaration>
162+
<Implementation>
163+
<ST><![CDATA[_mcForceControl(Axis:=_axisRef, Execute:=FALSE);
164+
]]></ST>
165+
</Implementation>
166+
</Method>
167+
<Method Name="Restore" Id="{19576e01-4841-4d6a-b398-2b5fa2d47cbe}">
168+
<Declaration><![CDATA[(*~
169+
<docu>
170+
<summary>
171+
Restores this instance to the Ready state. Can be called explicitly or from within one of the restore mechanisms.
172+
<para>
173+
See also <see cref="PlcDocu.TcoCore.TcoTask.AutoRestorable()"/>
174+
</para>
175+
</summary>
176+
</docu>
177+
~*)
178+
METHOD Restore : ITcoRestorable
179+
]]></Declaration>
180+
<Implementation>
181+
<ST><![CDATA[SUPER^.Restore();
182+
]]></ST>
183+
</Implementation>
184+
</Method>
185+
<Method Name="WhileError" Id="{0cbfbbef-bcc6-42db-b8c2-8a980d0b2822}">
186+
<Declaration><![CDATA[(*~
187+
<docu>
188+
<summary>
189+
Executes until task is in ```Error``` state .
190+
<para>
191+
Do not confuse with <see cref="PlcDocu.TcoCore.TcoTask.OnError()"/>, that is called just ones when '''Error''' state is entered.
192+
</para>
193+
</summary>
194+
</docu>
195+
~*)
196+
METHOD PROTECTED WhileError
197+
]]></Declaration>
198+
<Implementation>
199+
<ST><![CDATA[IF NOT _axisRef.Status.Error THEN
200+
DriveRef.SetErrorID(_mcForceControl.ErrorID);
201+
_mcForceControl(Axis:=_axisRef, Execute:=FALSE);
202+
END_IF
203+
]]></ST>
204+
</Implementation>
205+
</Method>
206+
</POU>
207+
</TcPlcObject>

Diff for: ‎src/TcoDrivesBeckhoff/src/XaeTcoDrivesBeckhoff/TcoDrivesBeckhoff/TcoDrivesBeckhoff.plcproj

+3
Original file line numberDiff line numberDiff line change
@@ -195,6 +195,9 @@
195195
<Compile Include="POUs\Prototypes\TcoDriveSimple\AxisTasks\SoEResetTask.TcPOU">
196196
<SubType>Code</SubType>
197197
</Compile>
198+
<Compile Include="POUs\Prototypes\TcoDriveSimple\MotionTasks\SetForceTask.TcPOU">
199+
<SubType>Code</SubType>
200+
</Compile>
198201
<Compile Include="POUs\Prototypes\TcoDriveSimple\MotionTasks\StopTask.TcPOU">
199202
<SubType>Code</SubType>
200203
</Compile>

0 commit comments

Comments
 (0)