[NEW-FEATURE] Add MC_TorqueControl (#722)

TcOpenGroup · Jun 12, 2024 · 803996a · 803996a
1 parent fc58c17
commit 803996a
Show file tree

Hide file tree

Showing 7 changed files with 167 additions and 44 deletions.
diff --git a/src/TcoDrivesBeckhoff/src/TcoDrivesBeckhoffConnector/Properties/Localizations.resx b/src/TcoDrivesBeckhoff/src/TcoDrivesBeckhoffConnector/Properties/Localizations.resx
@@ -228,6 +228,30 @@
   <data name="___Distance__" xml:space="preserve">
     <value>Distance</value>
   </data>
+  <data name="___Torque_Force__" xml:space="preserve">
+    <value>Torque/Force</value>
+  </data>
+  <data name="___Relative__" xml:space="preserve">
+    <value>Relative</value>
+  </data>
+  <data name="___Velocity_limit_high__" xml:space="preserve">
+    <value>Velocity limit high</value>
+  </data>
+  <data name="___Velocity_limit_low__" xml:space="preserve">
+    <value>Velocity limit low</value>
+  </data>
+  <data name="___Continuous_update__" xml:space="preserve">
+    <value>Continuous update</value>
+  </data>
+  <data name="___Torque_ramp__" xml:space="preserve">
+    <value>Torque ramp</value>
+  </data>
+  <data name="___Torque_Options__Enable_Manual_Torque_Start_value___" xml:space="preserve">
+    <value>Torque Options: Enable Manual Torque Start value </value>
+  </data>
+  <data name="___Torque_Options_Manual_Torque_Start_value__" xml:space="preserve">
+    <value>Torque Options:Manual Torque Start value</value>
+  </data>
   <data name="___Power_enable__" xml:space="preserve">
     <value>Power enable</value>
   </data>
@@ -303,6 +327,9 @@
   <data name="___Jogging__" xml:space="preserve">
     <value>Jogging</value>
   </data>
+  <data name="___Force_Torque__" xml:space="preserve">
+    <value>Force/Torque</value>
+  </data>
   <data name="___Move_velocity__" xml:space="preserve">
     <value>Move velocity</value>
   </data>
@@ -384,6 +411,9 @@
   <data name="___Hide_MoveVelocity_Tab__" xml:space="preserve">
     <value>Hide MoveVelocity Tab</value>
   </data>
+  <data name="___Hide_SetTorque_Tab__" xml:space="preserve">
+    <value>Hide SetTorque Tab</value>
+  </data>
   <data name="___Override_velocity_in_service______" xml:space="preserve">
     <value>Override velocity in service [%]</value>
   </data>

diff --git a/src/TcoDrivesBeckhoff/src/XaeTcoDrivesBeckhoff/TcoDrivesBeckhoff/DUTs/TcoAxisConfig.TcDUT b/src/TcoDrivesBeckhoff/src/XaeTcoDrivesBeckhoff/TcoDrivesBeckhoff/DUTs/TcoAxisConfig.TcDUT
@@ -24,6 +24,8 @@
         HideMoveModulo : BOOL;
         {attribute addProperty Name "<#Hide MoveVelocity Tab#>"}
         HideMoveVelocity : BOOL;
+		{attribute addProperty Name "<#Hide SetTorque Tab#>"}
+        HideSetTorque : BOOL;
 		{attribute addProperty Name "<#Override velocity in service [%]#>"}
 		OverrideInService : LREAL;
 

diff --git a/...khoff/TcoDrivesBeckhoff/POUs/Prototypes/TcoDriveSimple/MotionTasks/MoveVelocityTask.TcPOU b/...khoff/TcoDrivesBeckhoff/POUs/Prototypes/TcoDriveSimple/MotionTasks/MoveVelocityTask.TcPOU
@@ -90,6 +90,17 @@ END_VAR]]></Declaration>
         <ST><![CDATA[]]></ST>
       </Implementation>
     </Method>
+    <Property Name="InVelocity" Id="{d5a60ee2-c762-0ce1-0c1d-98b64e1b555a}">
+      <Declaration><![CDATA[PROPERTY PUBLIC InVelocity : BOOL]]></Declaration>
+      <Get Name="Get" Id="{d46b58c2-99eb-0f88-18dd-cc77bde30873}">
+        <Declaration><![CDATA[VAR
+END_VAR
+]]></Declaration>
+        <Implementation>
+          <ST><![CDATA[InVelocity :=_mcMoveVelocity.InVelocity;]]></ST>
+        </Implementation>
+      </Get>
+    </Property>
     <Method Name="InvokeWithParams" Id="{a12cf93b-f374-0fd0-3a32-7a0f0ba29d34}">
       <Declaration><![CDATA[METHOD INTERNAL InvokeWithParams : ITcoTaskStatus
 VAR_INPUT

diff --git a/...riveSimple/MotionTasks/SetForceTask.TcPOU → ...iveSimple/MotionTasks/SetTorqueTask.TcPOU b/...riveSimple/MotionTasks/SetForceTask.TcPOU → ...iveSimple/MotionTasks/SetTorqueTask.TcPOU
@@ -1,22 +1,37 @@
 <?xml version="1.0" encoding="utf-8"?>
 <TcPlcObject Version="1.1.0.1" ProductVersion="3.1.4024.12">
-  <POU Name="SetForceTask" Id="{202acf02-6d0d-4bca-9f77-4501493a923b}" SpecialFunc="None">
-    <Declaration><![CDATA[FUNCTION_BLOCK SetForceTask EXTENDS TcoTask IMPLEMENTS ITcoDriveSimple
+  <POU Name="SetTorqueTask" Id="{202acf02-6d0d-4bca-9f77-4501493a923b}" SpecialFunc="None">
+    <Declaration><![CDATA[FUNCTION_BLOCK SetTorqueTask EXTENDS TcoCore.TcoTask IMPLEMENTS ITcoDriveSimple
+VAR_INPUT
+	inIsServiceMode : BOOL;
+END_VAR
 VAR
-    {attribute addProperty Name "<#Force#>"}
-    _force_percent : LREAL;
-    {attribute addProperty Name "<#Velocity limit#>"}
-    _velocityLimit : LREAL;
+    {attribute addProperty Name "<#Torque/Force#>"}
+    _torque : LREAL;
+    {attribute addProperty Name "<#Relative#>"}
+    _relative : BOOL;
+    {attribute addProperty Name "<#Velocity limit high#>"}
+    _velocityLimitHigh : LREAL;
+	{attribute addProperty Name "<#Velocity limit low#>"}
+    _velocityLimitLow : LREAL;
+	{attribute addProperty Name "<#Continuous update#>"}
+    _continuousUpdate: BOOL:=TRUE;
     {attribute addProperty Name "<#Torque ramp#>"}
     _torqueRamp : LREAL;
+	{attribute addProperty Name "<#Torque Options: Enable Manual Torque Start value #>"}
+    _enableManualTorqueStartValue : BOOL;
+	{attribute addProperty Name "<#Torque Options:Manual Torque Start value#>"}
+    _manualTorqueStartValue : LREAL;
+   
+
 END_VAR
 VAR
     {attribute 'hide'}
     _axisRef : REFERENCE TO TcoAxisRef;
     {attribute 'hide'}
-    _mcForceControl : Tc2_MC2.MC_TorqueControl;
-    {attribute 'hide'}
-    _forceOptions : Tc2_MC2.ST_TorqueControlOptions;
+    _mcTorqueControl : Tc2_MC2.MC_TorqueControl;
+	    {attribute 'hide'}
+    _torqueOptions : Tc2_MC2.ST_TorqueControlOptions;
     {attribute 'hide'}
     _message : STRING(125);
 END_VAR
@@ -30,32 +45,40 @@ END_VAR
     END_IF
 END_IF
 
+
 IF Execute() THEN
-    _forceOptions.EnableManualTorqueStartValue := TRUE;
-    _forceOptions.ManualTorqueStartValue := _axisRef.NcToPlc.ActTorque;
-    _mcForceControl(
-        Axis:=_axisRef,
-        Execute:=NOT _mcForceControl.Busy,
-        Torque:=_force_percent,
-        BufferMode:=Tc2_MC2.MC_Aborting,
-        ContinuousUpdate:=TRUE,
-        Options:=_forceOptions,
-        TorqueRamp:=_torqueRamp,
-        VelocityLimitHigh:=_velocityLimit,
-        VelocityLimitLow:=_velocityLimit * -1,
-        Busy=>,
-        Active=>,
-        CommandAborted=>,
-        Error=>,
-        ErrorID=>
-    );
+	
+IF inIsServiceMode AND _enableManualTorqueStartValue THEN
+	_torqueOptions.EnableManualTorqueStartValue:=_enableManualTorqueStartValue;
+	_torqueOptions.ManualTorqueStartValue:=_manualTorqueStartValue;
+END_IF;
+
+_mcTorqueControl(
+	Axis:=_axisRef , 
+	Execute:=NOT _mcTorqueControl.Busy , 
+	ContinuousUpdate:=_continuousUpdate , 
+	Relative:= _relative, 
+	Torque:= _torque , 
+	TorqueRamp:= _torqueRamp , 
+	VelocityLimitHigh:= _velocityLimitHigh, 
+	VelocityLimitLow:= _velocityLimitLow, 
+	BufferMode:= Tc2_MC2.MC_Aborting , 
+	Options:=_torqueOptions , 
+	InTorque=> , 
+	Busy=> , 
+	Active=> , 
+	CommandAborted=> , 
+	Error=> , 
+	ErrorId=> );
+	
 
-    IF _mcForceControl.CommandAborted THEN
+
+    IF _mcTorqueControl.CommandAborted THEN
         THIS^.Abort();
     END_IF
 
-    DoneWhen(_mcForceControl.InTorque);
-    ThrowWhen(_mcForceControl.Error);
+    DoneWhen(_mcTorqueControl.InTorque);
+    ThrowWhen(_mcTorqueControl.Error);
 END_IF
 ]]></ST>
     </Implementation>
@@ -75,18 +98,38 @@ END_IF
         </Implementation>
       </Get>
     </Property>
+    <Property Name="InTorque" Id="{07615de5-82e8-0962-3b23-9a16f7555453}">
+      <Declaration><![CDATA[PROPERTY PUBLIC InTorque : BOOL]]></Declaration>
+      <Get Name="Get" Id="{c6d386ad-c5b5-0a81-1a00-81d8c3c290d3}">
+        <Declaration><![CDATA[VAR
+END_VAR
+]]></Declaration>
+        <Implementation>
+          <ST><![CDATA[InTorque :=_mcTorqueControl.InTorque;]]></ST>
+        </Implementation>
+      </Get>
+    </Property>
     <Method Name="InvokeWithParams" Id="{96f3ce16-2674-4918-9288-6d9cb1447a5e}">
       <Declaration><![CDATA[METHOD INTERNAL InvokeWithParams : ITcoTaskStatus
 VAR_INPUT
-    inForce_percent : LREAL;
-    inVelocityLimit : LREAL;
+    inTorque : LREAL;
+	inRelative : BOOL;
+    inVelocityLimitHigh : LREAL;
+    inVelocityLimitLow : LREAL;
+    inContinuousUpdate: BOOL:=TRUE;
     inTorqueRamp : LREAL;
+    inTorqueOptions : Tc2_MC2.ST_TorqueControlOptions;
 END_VAR
-]]></Declaration>
+
+	]]></Declaration>
       <Implementation>
-        <ST><![CDATA[_force_percent := inForce_percent;
-_velocityLimit := inVelocityLimit;
+        <ST><![CDATA[_torque := inTorque ;
+_relative := inRelative ;
+_velocityLimitHigh := inVelocityLimitHigh;
+_velocityLimitLow := inVelocityLimitLow;
+_continuousUpdate:=inContinuousUpdate;
 _torqueRamp := inTorqueRamp;
+_torqueOptions:=inTorqueOptions;
 InvokeWithParams := THIS^.Invoke();
 ]]></ST>
       </Implementation>
@@ -111,7 +154,7 @@ METHOD PROTECTED OnAbort
 END_IF
 
 THIS^.DriveRef.Stop(-1, -1);
-_mcForceControl(Axis:=_axisRef, Execute:=FALSE);
+_mcTorqueControl(Axis:=_axisRef, Execute:=FALSE);
 THIS^.DriveRef._movementState.Restore();
 ]]></ST>
       </Implementation>
@@ -127,7 +170,7 @@ THIS^.DriveRef._movementState.Restore();
 METHOD PROTECTED OnDone
 ]]></Declaration>
       <Implementation>
-        <ST><![CDATA[_mcForceControl(Axis:=_axisRef, Execute:=FALSE);
+        <ST><![CDATA[_mcTorqueControl(Axis:=_axisRef, Execute:=FALSE);
 THIS^.DriveRef._movementState.Restore();
 ]]></ST>
       </Implementation>
@@ -152,15 +195,15 @@ IF NOT _axisRef.Status.Error THEN
     DriveRef.SetErrorID(DriveRef._axisStatus.ErrorId);
 END_IF
 
-_mcForceControl(Axis:=_axisRef, Execute:=FALSE);
+_mcTorqueControl(Axis:=_axisRef, Execute:=FALSE);
 ]]></ST>
       </Implementation>
     </Method>
     <Method Name="OnStart" Id="{be299f11-c5f4-4588-8158-9ce4a7891dec}">
       <Declaration><![CDATA[METHOD PROTECTED OnStart
 ]]></Declaration>
       <Implementation>
-        <ST><![CDATA[_mcForceControl(Axis:=_axisRef, Execute:=FALSE);
+        <ST><![CDATA[_mcTorqueControl(Axis:=_axisRef, Execute:=FALSE);
 ]]></ST>
       </Implementation>
     </Method>
@@ -197,8 +240,8 @@ METHOD PROTECTED WhileError
 ]]></Declaration>
       <Implementation>
         <ST><![CDATA[IF NOT _axisRef.Status.Error THEN
-    DriveRef.SetErrorID(_mcForceControl.ErrorID);
-    _mcForceControl(Axis:=_axisRef, Execute:=FALSE);
+    DriveRef.SetErrorID(_mcTorqueControl.ErrorID);
+    _mcTorqueControl(Axis:=_axisRef, Execute:=FALSE);
 END_IF
 ]]></ST>
       </Implementation>

diff --git a/...aeTcoDrivesBeckhoff/TcoDrivesBeckhoff/POUs/Prototypes/TcoDriveSimple/TcoDriveSimple.TcPOU b/...aeTcoDrivesBeckhoff/TcoDrivesBeckhoff/POUs/Prototypes/TcoDriveSimple/TcoDriveSimple.TcPOU
@@ -48,6 +48,8 @@ VAR
     _homeTask : HomeTask(THIS^);
     {attribute addProperty Name "<#Jogging#>"}
     _jog : Jog(THIS^);
+	{attribute addProperty Name "<#Force/Torque#>"}
+    _torqueTask : SetTorqueTask(THIS^);
     {attribute addProperty Name "<#Move velocity#>"}
     _moveVelocityTask : MoveVelocityTask(THIS^);
     {attribute addProperty Name "<#Stop#>"}
@@ -75,6 +77,7 @@ END_IF
 _axisRef REF= inoAxisRef;
 
 
+
 _mcReadStatus(Axis := _axisRef, Enable := TRUE);
 UpdateAxisStatus();
 
@@ -103,6 +106,7 @@ _haltTask();
 _moveRelativeTask();
 _moveAbsoluteTask();
 _moveModuloTask();
+_torqueTask(inIsServiceMode:= THIS^.IsService);
 
 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;
 
@@ -463,7 +467,8 @@ _haltTask.Restore();
 _moveRelativeTask.Restore();
 _moveAbsoluteTask.Restore();
 _moveModuloTask.Restore();
-_movementState.Restore();]]></ST>
+_movementState.Restore();
+_torqueTask.Restore();]]></ST>
       </Implementation>
     </Method>
     <Method Name="ServiceMode" Id="{f2dcab5e-74c4-0882-12e2-82e083006063}">
@@ -513,6 +518,38 @@ END_VAR]]></Declaration>
         </Implementation>
       </Get>
     </Property>
+    <Method Name="SetTorque" Id="{a5a969d5-5c13-0a76-173a-6e38461490a0}" FolderPath="Motion\">
+      <Declaration><![CDATA[METHOD SetTorque : ITcoTaskStatus
+VAR_INPUT
+   	inTorque : LREAL;
+	inRelative : BOOL;
+    inVelocityLimitHigh : LREAL;
+    inVelocityLimitLow : LREAL;
+    inContinuousUpdate: BOOL:=TRUE;
+    inTorqueRamp : LREAL;
+    inTorqueOptions : Tc2_MC2.ST_TorqueControlOptions;
+END_VAR]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[SetTorque := THIS^._torqueTask.InvokeWithParams(
+	inTorque:=inTorque , 
+	inRelative:=inRelative , 
+	inVelocityLimitHigh:=inVelocityLimitHigh , 
+	inVelocityLimitLow:= inVelocityLimitLow, 
+	inContinuousUpdate:= inContinuousUpdate , 
+	inTorqueRamp:= inTorqueRamp , 
+	inTorqueOptions:= inTorqueOptions);
+	]]></ST>
+      </Implementation>
+    </Method>
+    <Property Name="SetTorqueTask" Id="{690cf8bc-ff65-0398-22a3-b3cc663a8c89}" FolderPath="Motion\">
+      <Declaration><![CDATA[PROPERTY SetTorqueTask : TcoCore.ITcoTaskStatus]]></Declaration>
+      <Get Name="Get" Id="{50a39b8e-1dec-04de-1f90-fa63cc279752}">
+        <Declaration><![CDATA[]]></Declaration>
+        <Implementation>
+          <ST><![CDATA[SetTorqueTask := _torqueTask;]]></ST>
+        </Implementation>
+      </Get>
+    </Property>
     <Method Name="SoEReset" Id="{7430d658-985b-0e2d-3ddc-c2f5991b2139}" FolderPath="Axis\">
       <Declaration><![CDATA[METHOD SoEReset : ITcoTaskStatus]]></Declaration>
       <Implementation>

diff --git a/src/TcoDrivesBeckhoff/src/XaeTcoDrivesBeckhoff/TcoDrivesBeckhoff/TcoDrivesBeckhoff.plcproj b/src/TcoDrivesBeckhoff/src/XaeTcoDrivesBeckhoff/TcoDrivesBeckhoff/TcoDrivesBeckhoff.plcproj
@@ -195,7 +195,7 @@
     <Compile Include="POUs\Prototypes\TcoDriveSimple\AxisTasks\SoEResetTask.TcPOU">
       <SubType>Code</SubType>
     </Compile>
-    <Compile Include="POUs\Prototypes\TcoDriveSimple\MotionTasks\SetForceTask.TcPOU">
+    <Compile Include="POUs\Prototypes\TcoDriveSimple\MotionTasks\SetTorqueTask.TcPOU">
       <SubType>Code</SubType>
     </Compile>
     <Compile Include="POUs\Prototypes\TcoDriveSimple\MotionTasks\StopTask.TcPOU">

diff --git a/src/TcoDrivesBeckhoff/src/XaeTcoDrivesBeckhoff/XaeTcoDrivesBeckhoff.tsproj b/src/TcoDrivesBeckhoff/src/XaeTcoDrivesBeckhoff/XaeTcoDrivesBeckhoff.tsproj
@@ -2797,7 +2797,7 @@ External Setpoint Generation:
 				</Instance>
 			</Project>
 			<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">
-				<Instance Id="#x08502040" TcSmClass="TComPlcObjDef" KeepUnrestoredLinks="2" TmcPath="TcoDrivesBeckhoffTests\TcoDrivesBeckhoffTests.tmc" TmcHash="{A0657F7F-995F-11BE-8CAB-D41B21C61701}">
+				<Instance Id="#x08502040" TcSmClass="TComPlcObjDef" KeepUnrestoredLinks="2" TmcPath="TcoDrivesBeckhoffTests\TcoDrivesBeckhoffTests.tmc" TmcHash="{B46E16B9-76AC-237B-74B4-FC6CDB42E07E}">
 					<Name>TcoDrivesBeckhoffTests Instance</Name>
 					<CLSID ClassFactory="TcPlc30">{08500001-0000-0000-F000-000000000064}</CLSID>
 					<Vars VarGrpType="1">