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

Commit 803996a

Browse files
github-actions[bot]github-actions[bot]
authored
[NEW-FEATURE] Add MC_TorqueControl (#722)
1 parent fc58c17 commit 803996a

File tree

7 files changed

+167
-44
lines changed

7 files changed

+167
-44
lines changed

‎src/TcoDrivesBeckhoff/src/TcoDrivesBeckhoffConnector/Properties/Localizations.resx

+30
Original file line numberDiff line numberDiff line change
@@ -228,6 +228,30 @@
228228
<data name="___Distance__" xml:space="preserve">
229229
<value>Distance</value>
230230
</data>
231+
<data name="___Torque_Force__" xml:space="preserve">
232+
<value>Torque/Force</value>
233+
</data>
234+
<data name="___Relative__" xml:space="preserve">
235+
<value>Relative</value>
236+
</data>
237+
<data name="___Velocity_limit_high__" xml:space="preserve">
238+
<value>Velocity limit high</value>
239+
</data>
240+
<data name="___Velocity_limit_low__" xml:space="preserve">
241+
<value>Velocity limit low</value>
242+
</data>
243+
<data name="___Continuous_update__" xml:space="preserve">
244+
<value>Continuous update</value>
245+
</data>
246+
<data name="___Torque_ramp__" xml:space="preserve">
247+
<value>Torque ramp</value>
248+
</data>
249+
<data name="___Torque_Options__Enable_Manual_Torque_Start_value___" xml:space="preserve">
250+
<value>Torque Options: Enable Manual Torque Start value </value>
251+
</data>
252+
<data name="___Torque_Options_Manual_Torque_Start_value__" xml:space="preserve">
253+
<value>Torque Options:Manual Torque Start value</value>
254+
</data>
231255
<data name="___Power_enable__" xml:space="preserve">
232256
<value>Power enable</value>
233257
</data>
@@ -303,6 +327,9 @@
303327
<data name="___Jogging__" xml:space="preserve">
304328
<value>Jogging</value>
305329
</data>
330+
<data name="___Force_Torque__" xml:space="preserve">
331+
<value>Force/Torque</value>
332+
</data>
306333
<data name="___Move_velocity__" xml:space="preserve">
307334
<value>Move velocity</value>
308335
</data>
@@ -384,6 +411,9 @@
384411
<data name="___Hide_MoveVelocity_Tab__" xml:space="preserve">
385412
<value>Hide MoveVelocity Tab</value>
386413
</data>
414+
<data name="___Hide_SetTorque_Tab__" xml:space="preserve">
415+
<value>Hide SetTorque Tab</value>
416+
</data>
387417
<data name="___Override_velocity_in_service______" xml:space="preserve">
388418
<value>Override velocity in service [%]</value>
389419
</data>

‎src/TcoDrivesBeckhoff/src/XaeTcoDrivesBeckhoff/TcoDrivesBeckhoff/DUTs/TcoAxisConfig.TcDUT

+2
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,8 @@
2424
HideMoveModulo : BOOL;
2525
{attribute addProperty Name "<#Hide MoveVelocity Tab#>"}
2626
HideMoveVelocity : BOOL;
27+
{attribute addProperty Name "<#Hide SetTorque Tab#>"}
28+
HideSetTorque : BOOL;
2729
{attribute addProperty Name "<#Override velocity in service [%]#>"}
2830
OverrideInService : LREAL;
2931

‎src/TcoDrivesBeckhoff/src/XaeTcoDrivesBeckhoff/TcoDrivesBeckhoff/POUs/Prototypes/TcoDriveSimple/MotionTasks/MoveVelocityTask.TcPOU

+11
Original file line numberDiff line numberDiff line change
@@ -90,6 +90,17 @@ END_VAR]]></Declaration>
9090
<ST><![CDATA[]]></ST>
9191
</Implementation>
9292
</Method>
93+
<Property Name="InVelocity" Id="{d5a60ee2-c762-0ce1-0c1d-98b64e1b555a}">
94+
<Declaration><![CDATA[PROPERTY PUBLIC InVelocity : BOOL]]></Declaration>
95+
<Get Name="Get" Id="{d46b58c2-99eb-0f88-18dd-cc77bde30873}">
96+
<Declaration><![CDATA[VAR
97+
END_VAR
98+
]]></Declaration>
99+
<Implementation>
100+
<ST><![CDATA[InVelocity :=_mcMoveVelocity.InVelocity;]]></ST>
101+
</Implementation>
102+
</Get>
103+
</Property>
93104
<Method Name="InvokeWithParams" Id="{a12cf93b-f374-0fd0-3a32-7a0f0ba29d34}">
94105
<Declaration><![CDATA[METHOD INTERNAL InvokeWithParams : ITcoTaskStatus
95106
VAR_INPUT

‎src/TcoDrivesBeckhoff/src/XaeTcoDrivesBeckhoff/TcoDrivesBeckhoff/POUs/Prototypes/TcoDriveSimple/MotionTasks/SetForceTask.TcPOU ‎src/TcoDrivesBeckhoff/src/XaeTcoDrivesBeckhoff/TcoDrivesBeckhoff/POUs/Prototypes/TcoDriveSimple/MotionTasks/SetTorqueTask.TcPOU

+84-41
Original file line numberDiff line numberDiff line change
@@ -1,22 +1,37 @@
11
<?xml version="1.0" encoding="utf-8"?>
22
<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
3+
<POU Name="SetTorqueTask" Id="{202acf02-6d0d-4bca-9f77-4501493a923b}" SpecialFunc="None">
4+
<Declaration><![CDATA[FUNCTION_BLOCK SetTorqueTask EXTENDS TcoCore.TcoTask IMPLEMENTS ITcoDriveSimple
5+
VAR_INPUT
6+
inIsServiceMode : BOOL;
7+
END_VAR
58
VAR
6-
{attribute addProperty Name "<#Force#>"}
7-
_force_percent : LREAL;
8-
{attribute addProperty Name "<#Velocity limit#>"}
9-
_velocityLimit : LREAL;
9+
{attribute addProperty Name "<#Torque/Force#>"}
10+
_torque : LREAL;
11+
{attribute addProperty Name "<#Relative#>"}
12+
_relative : BOOL;
13+
{attribute addProperty Name "<#Velocity limit high#>"}
14+
_velocityLimitHigh : LREAL;
15+
{attribute addProperty Name "<#Velocity limit low#>"}
16+
_velocityLimitLow : LREAL;
17+
{attribute addProperty Name "<#Continuous update#>"}
18+
_continuousUpdate: BOOL:=TRUE;
1019
{attribute addProperty Name "<#Torque ramp#>"}
1120
_torqueRamp : LREAL;
21+
{attribute addProperty Name "<#Torque Options: Enable Manual Torque Start value #>"}
22+
_enableManualTorqueStartValue : BOOL;
23+
{attribute addProperty Name "<#Torque Options:Manual Torque Start value#>"}
24+
_manualTorqueStartValue : LREAL;
25+
26+
1227
END_VAR
1328
VAR
1429
{attribute 'hide'}
1530
_axisRef : REFERENCE TO TcoAxisRef;
1631
{attribute 'hide'}
17-
_mcForceControl : Tc2_MC2.MC_TorqueControl;
18-
{attribute 'hide'}
19-
_forceOptions : Tc2_MC2.ST_TorqueControlOptions;
32+
_mcTorqueControl : Tc2_MC2.MC_TorqueControl;
33+
{attribute 'hide'}
34+
_torqueOptions : Tc2_MC2.ST_TorqueControlOptions;
2035
{attribute 'hide'}
2136
_message : STRING(125);
2237
END_VAR
@@ -30,32 +45,40 @@ END_VAR
3045
END_IF
3146
END_IF
3247
48+
3349
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-
);
50+
51+
IF inIsServiceMode AND _enableManualTorqueStartValue THEN
52+
_torqueOptions.EnableManualTorqueStartValue:=_enableManualTorqueStartValue;
53+
_torqueOptions.ManualTorqueStartValue:=_manualTorqueStartValue;
54+
END_IF;
55+
56+
_mcTorqueControl(
57+
Axis:=_axisRef ,
58+
Execute:=NOT _mcTorqueControl.Busy ,
59+
ContinuousUpdate:=_continuousUpdate ,
60+
Relative:= _relative,
61+
Torque:= _torque ,
62+
TorqueRamp:= _torqueRamp ,
63+
VelocityLimitHigh:= _velocityLimitHigh,
64+
VelocityLimitLow:= _velocityLimitLow,
65+
BufferMode:= Tc2_MC2.MC_Aborting ,
66+
Options:=_torqueOptions ,
67+
InTorque=> ,
68+
Busy=> ,
69+
Active=> ,
70+
CommandAborted=> ,
71+
Error=> ,
72+
ErrorId=> );
73+
5274
53-
IF _mcForceControl.CommandAborted THEN
75+
76+
IF _mcTorqueControl.CommandAborted THEN
5477
THIS^.Abort();
5578
END_IF
5679
57-
DoneWhen(_mcForceControl.InTorque);
58-
ThrowWhen(_mcForceControl.Error);
80+
DoneWhen(_mcTorqueControl.InTorque);
81+
ThrowWhen(_mcTorqueControl.Error);
5982
END_IF
6083
]]></ST>
6184
</Implementation>
@@ -75,18 +98,38 @@ END_IF
7598
</Implementation>
7699
</Get>
77100
</Property>
101+
<Property Name="InTorque" Id="{07615de5-82e8-0962-3b23-9a16f7555453}">
102+
<Declaration><![CDATA[PROPERTY PUBLIC InTorque : BOOL]]></Declaration>
103+
<Get Name="Get" Id="{c6d386ad-c5b5-0a81-1a00-81d8c3c290d3}">
104+
<Declaration><![CDATA[VAR
105+
END_VAR
106+
]]></Declaration>
107+
<Implementation>
108+
<ST><![CDATA[InTorque :=_mcTorqueControl.InTorque;]]></ST>
109+
</Implementation>
110+
</Get>
111+
</Property>
78112
<Method Name="InvokeWithParams" Id="{96f3ce16-2674-4918-9288-6d9cb1447a5e}">
79113
<Declaration><![CDATA[METHOD INTERNAL InvokeWithParams : ITcoTaskStatus
80114
VAR_INPUT
81-
inForce_percent : LREAL;
82-
inVelocityLimit : LREAL;
115+
inTorque : LREAL;
116+
inRelative : BOOL;
117+
inVelocityLimitHigh : LREAL;
118+
inVelocityLimitLow : LREAL;
119+
inContinuousUpdate: BOOL:=TRUE;
83120
inTorqueRamp : LREAL;
121+
inTorqueOptions : Tc2_MC2.ST_TorqueControlOptions;
84122
END_VAR
85-
]]></Declaration>
123+
124+
]]></Declaration>
86125
<Implementation>
87-
<ST><![CDATA[_force_percent := inForce_percent;
88-
_velocityLimit := inVelocityLimit;
126+
<ST><![CDATA[_torque := inTorque ;
127+
_relative := inRelative ;
128+
_velocityLimitHigh := inVelocityLimitHigh;
129+
_velocityLimitLow := inVelocityLimitLow;
130+
_continuousUpdate:=inContinuousUpdate;
89131
_torqueRamp := inTorqueRamp;
132+
_torqueOptions:=inTorqueOptions;
90133
InvokeWithParams := THIS^.Invoke();
91134
]]></ST>
92135
</Implementation>
@@ -111,7 +154,7 @@ METHOD PROTECTED OnAbort
111154
END_IF
112155
113156
THIS^.DriveRef.Stop(-1, -1);
114-
_mcForceControl(Axis:=_axisRef, Execute:=FALSE);
157+
_mcTorqueControl(Axis:=_axisRef, Execute:=FALSE);
115158
THIS^.DriveRef._movementState.Restore();
116159
]]></ST>
117160
</Implementation>
@@ -127,7 +170,7 @@ THIS^.DriveRef._movementState.Restore();
127170
METHOD PROTECTED OnDone
128171
]]></Declaration>
129172
<Implementation>
130-
<ST><![CDATA[_mcForceControl(Axis:=_axisRef, Execute:=FALSE);
173+
<ST><![CDATA[_mcTorqueControl(Axis:=_axisRef, Execute:=FALSE);
131174
THIS^.DriveRef._movementState.Restore();
132175
]]></ST>
133176
</Implementation>
@@ -152,15 +195,15 @@ IF NOT _axisRef.Status.Error THEN
152195
DriveRef.SetErrorID(DriveRef._axisStatus.ErrorId);
153196
END_IF
154197
155-
_mcForceControl(Axis:=_axisRef, Execute:=FALSE);
198+
_mcTorqueControl(Axis:=_axisRef, Execute:=FALSE);
156199
]]></ST>
157200
</Implementation>
158201
</Method>
159202
<Method Name="OnStart" Id="{be299f11-c5f4-4588-8158-9ce4a7891dec}">
160203
<Declaration><![CDATA[METHOD PROTECTED OnStart
161204
]]></Declaration>
162205
<Implementation>
163-
<ST><![CDATA[_mcForceControl(Axis:=_axisRef, Execute:=FALSE);
206+
<ST><![CDATA[_mcTorqueControl(Axis:=_axisRef, Execute:=FALSE);
164207
]]></ST>
165208
</Implementation>
166209
</Method>
@@ -197,8 +240,8 @@ METHOD PROTECTED WhileError
197240
]]></Declaration>
198241
<Implementation>
199242
<ST><![CDATA[IF NOT _axisRef.Status.Error THEN
200-
DriveRef.SetErrorID(_mcForceControl.ErrorID);
201-
_mcForceControl(Axis:=_axisRef, Execute:=FALSE);
243+
DriveRef.SetErrorID(_mcTorqueControl.ErrorID);
244+
_mcTorqueControl(Axis:=_axisRef, Execute:=FALSE);
202245
END_IF
203246
]]></ST>
204247
</Implementation>

‎src/TcoDrivesBeckhoff/src/XaeTcoDrivesBeckhoff/TcoDrivesBeckhoff/POUs/Prototypes/TcoDriveSimple/TcoDriveSimple.TcPOU

+38-1
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,8 @@ VAR
4848
_homeTask : HomeTask(THIS^);
4949
{attribute addProperty Name "<#Jogging#>"}
5050
_jog : Jog(THIS^);
51+
{attribute addProperty Name "<#Force/Torque#>"}
52+
_torqueTask : SetTorqueTask(THIS^);
5153
{attribute addProperty Name "<#Move velocity#>"}
5254
_moveVelocityTask : MoveVelocityTask(THIS^);
5355
{attribute addProperty Name "<#Stop#>"}
@@ -75,6 +77,7 @@ END_IF
7577
_axisRef REF= inoAxisRef;
7678
7779
80+
7881
_mcReadStatus(Axis := _axisRef, Enable := TRUE);
7982
UpdateAxisStatus();
8083
@@ -103,6 +106,7 @@ _haltTask();
103106
_moveRelativeTask();
104107
_moveAbsoluteTask();
105108
_moveModuloTask();
109+
_torqueTask(inIsServiceMode:= THIS^.IsService);
106110
107111
IF(_axisStatus.Error) THEN Messenger.Error(_sb.Clear().Append('<#Drive error no #>').Append(UDINT_TO_STRING(_axisStatus.ErrorId)).Append(' |[[0]AdvancedDiagnosticMessage]|').Append('.').Append('For details see the service mode').ToString()); END_IF;
108112
@@ -463,7 +467,8 @@ _haltTask.Restore();
463467
_moveRelativeTask.Restore();
464468
_moveAbsoluteTask.Restore();
465469
_moveModuloTask.Restore();
466-
_movementState.Restore();]]></ST>
470+
_movementState.Restore();
471+
_torqueTask.Restore();]]></ST>
467472
</Implementation>
468473
</Method>
469474
<Method Name="ServiceMode" Id="{f2dcab5e-74c4-0882-12e2-82e083006063}">
@@ -513,6 +518,38 @@ END_VAR]]></Declaration>
513518
</Implementation>
514519
</Get>
515520
</Property>
521+
<Method Name="SetTorque" Id="{a5a969d5-5c13-0a76-173a-6e38461490a0}" FolderPath="Motion\">
522+
<Declaration><![CDATA[METHOD SetTorque : ITcoTaskStatus
523+
VAR_INPUT
524+
inTorque : LREAL;
525+
inRelative : BOOL;
526+
inVelocityLimitHigh : LREAL;
527+
inVelocityLimitLow : LREAL;
528+
inContinuousUpdate: BOOL:=TRUE;
529+
inTorqueRamp : LREAL;
530+
inTorqueOptions : Tc2_MC2.ST_TorqueControlOptions;
531+
END_VAR]]></Declaration>
532+
<Implementation>
533+
<ST><![CDATA[SetTorque := THIS^._torqueTask.InvokeWithParams(
534+
inTorque:=inTorque ,
535+
inRelative:=inRelative ,
536+
inVelocityLimitHigh:=inVelocityLimitHigh ,
537+
inVelocityLimitLow:= inVelocityLimitLow,
538+
inContinuousUpdate:= inContinuousUpdate ,
539+
inTorqueRamp:= inTorqueRamp ,
540+
inTorqueOptions:= inTorqueOptions);
541+
]]></ST>
542+
</Implementation>
543+
</Method>
544+
<Property Name="SetTorqueTask" Id="{690cf8bc-ff65-0398-22a3-b3cc663a8c89}" FolderPath="Motion\">
545+
<Declaration><![CDATA[PROPERTY SetTorqueTask : TcoCore.ITcoTaskStatus]]></Declaration>
546+
<Get Name="Get" Id="{50a39b8e-1dec-04de-1f90-fa63cc279752}">
547+
<Declaration><![CDATA[]]></Declaration>
548+
<Implementation>
549+
<ST><![CDATA[SetTorqueTask := _torqueTask;]]></ST>
550+
</Implementation>
551+
</Get>
552+
</Property>
516553
<Method Name="SoEReset" Id="{7430d658-985b-0e2d-3ddc-c2f5991b2139}" FolderPath="Axis\">
517554
<Declaration><![CDATA[METHOD SoEReset : ITcoTaskStatus]]></Declaration>
518555
<Implementation>

‎src/TcoDrivesBeckhoff/src/XaeTcoDrivesBeckhoff/TcoDrivesBeckhoff/TcoDrivesBeckhoff.plcproj

+1-1
Original file line numberDiff line numberDiff line change
@@ -195,7 +195,7 @@
195195
<Compile Include="POUs\Prototypes\TcoDriveSimple\AxisTasks\SoEResetTask.TcPOU">
196196
<SubType>Code</SubType>
197197
</Compile>
198-
<Compile Include="POUs\Prototypes\TcoDriveSimple\MotionTasks\SetForceTask.TcPOU">
198+
<Compile Include="POUs\Prototypes\TcoDriveSimple\MotionTasks\SetTorqueTask.TcPOU">
199199
<SubType>Code</SubType>
200200
</Compile>
201201
<Compile Include="POUs\Prototypes\TcoDriveSimple\MotionTasks\StopTask.TcPOU">

‎src/TcoDrivesBeckhoff/src/XaeTcoDrivesBeckhoff/XaeTcoDrivesBeckhoff.tsproj

+1-1
Original file line numberDiff line numberDiff line change
@@ -2797,7 +2797,7 @@ External Setpoint Generation:
27972797
</Instance>
27982798
</Project>
27992799
<Project GUID="{4F0E1158-49EA-45D6-89E6-C1A4D3AF626F}" Name="TcoDrivesBeckhoffTests" PrjFilePath="TcoDrivesBeckhoffTests\TcoDrivesBeckhoffTests.plcproj" TmcFilePath="TcoDrivesBeckhoffTests\TcoDrivesBeckhoffTests.tmc" ReloadTmc="true" AmsPort="852" FileArchiveSettings="#x000e" SymbolicMapping="true">
2800-
<Instance Id="#x08502040" TcSmClass="TComPlcObjDef" KeepUnrestoredLinks="2" TmcPath="TcoDrivesBeckhoffTests\TcoDrivesBeckhoffTests.tmc" TmcHash="{A0657F7F-995F-11BE-8CAB-D41B21C61701}">
2800+
<Instance Id="#x08502040" TcSmClass="TComPlcObjDef" KeepUnrestoredLinks="2" TmcPath="TcoDrivesBeckhoffTests\TcoDrivesBeckhoffTests.tmc" TmcHash="{B46E16B9-76AC-237B-74B4-FC6CDB42E07E}">
28012801
<Name>TcoDrivesBeckhoffTests Instance</Name>
28022802
<CLSID ClassFactory="TcPlc30">{08500001-0000-0000-F000-000000000064}</CLSID>
28032803
<Vars VarGrpType="1">

0 commit comments

Comments
 (0)