力覺功能程式範例

以下說明結合力覺功能之簡易操作的範例。

注意

本範例中所述的參數僅為參考值。

請注意，已使用相對穩定的參數，但操作可能不會成功，或在特定操作條件下動作時可能會發生振動，且可能需要調整參數。

此外，為了方便說明，會使用緩慢穩定的參數。如需加快操作速度，參數需進行調整。

按壓操作

以下說明使用目標力在固定方向中執行按壓操作的程式範例。

此範例操作將工件移動至接觸位置上方10 [mm]的位置，並使用力控制功能以20 [N]的力將機械臂握持的工件按壓至工作台上。

同時，使用力覺觸發功能以在操作期間監控過度力(100 [N]以上)，並偵測過度力是否導致錯誤發生。

使用力動作限制功能，在作業中進行意外移動(20 [mm]以上)時，作為工作臺不存在的異常狀態，停止機器人的動作。

此外，使用力覺監控功能以在操作完成後測量力覺感應器值，並測量操作期間的最大施加力。
工具座標系設定於末端夾具，末端夾具的前方為Tlz軸方向。

範例編程

Function PressSample_Main
  Real rVar(8)
  Integer iVar
  Motor On
  Go P0                                 '前往操作開始位置
  PressSample_PropertySetting           '設定屬性
  FSet FS1.Reset                        '重置力覺感應器
  Trap 1, FT1 Call PressSample_EHandle  '開始監控過度力
  FSet FM1.PeakForceClear, True, True, True, False, False, False, True, False
    '開始計算峰值
  Till FMR1                             '設定移動超出預期範圍時的動作停止條件
  FCKeep FC1 Till, 10                   '執行力控制功能10秒
  Print "Motion End"
  FGet FM1.Forces, rVar()               '讀取力覺感應器值
  Print "Force Fz：", rVar(FG_FZ), ", Fmag：", rVar(FG_FMAG)
  FGet FM1.PeakForces, rVar()           '讀取峰值
  Print "PeakForce Fz：", rVar(FG_FZ), ", Fmag：", rVar(FG_FMAG)
  FGet FMR1.Triggered, iVar             '讀取動作限制的結果
  If iVar = True Then                   '若已限制則顯示Overrun錯誤
    Print "Overrun Error"
  EndIf
Fend

Function PressSample_PropertySetting
  FSet FCS1.Position, 0, 0, 30          '力座標系的原點位於Z30 mm
  FSet FCS1.Orientation, FG_TOOL        '方向與工具座標系一致

  FSet FC1.CoordinateSystem, FCS1       '指定已定義的1號力座標
  FSet FC1.Enabled, False, False, True, False, False, False
    '僅為Fz方向啟動力控制功能。
  FSet FC1.Fz_TargetForce, -20          '按壓20N
  FSet FC1.Fz_Spring, 0                 '虛擬彈性係數為0
  FSet FC1.Fz_Damper, 10                '虛擬阻尼係數為10
  FSet FC1.Fz_Mass, 10                  '虛擬質量係數為10

  FSet FT1.ForceSensor, 1               '指定1號力覺感應器
  FSet FT1.CoordinateSystem, FCS1       '指定已定義的1號力座標
  FSet FT1.TriggerMode, FG_FORCE        '監控力
  FSet FT1.Fmag_Axes, FG_XYZ
  FSet FT1.Enabled, False, False, False, False, False, False, True, False
    '僅啟動Fmag
  FSet FT1.Fmag_Polarity, FG_OUT        ' 觸發器在超出閾值範圍時偵測
  FSet FT1.Fmag_Levels, 0, 100          'Fmag的範圍為0~100

  FSet FM1.ForceSensor, 1               '指定1號力覺感應器
  FSet FM1.CoordinateSystem, FCS1       '指定已定義的1號力座標

  FSet FMR1.TriggerMode, FG_REL_TOOL    '指定工具相對模式
  FSet FMR1.PosEnabled, False, False, True, False, False
    '僅Z方向有效
  FSet FMR1.PosZ_Levels, -20, 20        'Z的範圍為-20mm~20mm
  FSet FMR1.PosZ_Polarity, FG_OUT       '超過閾值範圍時限制有效
  FSet FMR1.ForceSensor, 1              '指定1號力覺感應器
  FSet FMR1.CoordinateSystem, FCS1      '指定已定義的1號力座標
Fend

Function PressSample_EHandle
  Real rVar(8)
  FGet FM1.PeakForces, rVar()           '讀取峰值
  Print "Error Handle"
  Print "PeakForce Fz：", rVar(FG_FZ), ", Fmag：", rVar(FG_FMAG)
  AbortMotion All                       '停止機器人動作，並視為錯誤狀態
Fend

說明

執行PressSample_Main函數後，機器人將往作業開始位置移動。
呼叫PressSample_PropertySetting並執行各屬性的設定。
不過，也可在GUI的「力覺編輯器」中預先進行屬性設定。屆時便不需要呼叫PressSample_PropertySetting。
設定力覺座標物件。如為力覺座標系，請在範例中指定Z30 [mm]位置，以指定工具座標系中的工件邊緣位置。方向與工具座標系的方向相同。
設定力覺控制物件。指定設為座標系且執行力控制功能的FCS1。因在正FZ方向上執行按壓操作，因此請指定目標力的負值。設定虛擬彈性係數、虛擬阻尼係數及虛擬質量係數。在此範例中，虛擬彈性係數的設定值為「0」，因此機器人未具備虛擬彈性，且會在達到目標力前繼續移動。此外，針對虛擬阻尼係數及虛擬質量係數使用穩定參數。如需使操作速度較快，請逐漸減少數值以進行調整。不過，減少數值會增加力過衝。
設定力覺觸發物件。指定要使用的力覺感應器編號及執行力覺觸發功能的座標系。在TriggerMode屬性中指定力值以監控過度力。因使用所有Fx至Fz來計算要監控的合力，因此請指定X、Y及Z。在Fmag範圍中指定0至100 [N]，將過度力設為100 [N]，並設定監控數值是否超出此範圍。
設定力覺監控物件。指定用於測量的力覺感應器編號及座標系。
設定力覺動作限制物件。機器人通過 (a) 和 (b) 設置沿工具座標系的 +Z 方向移動。在此範例中，如果移動超過 20mm，則運行以沒有檢測到要按壓物件的設置。為TriggerMode模式指定工具相對模式以檢測工具座標系的移動距離。僅啟用Enabled的PosZ以檢測Z方向。將-20~20[mm]指定為Z方向範圍，並將其設置為監視超出此範圍的範圍。
在使用力覺功能前重置力覺感應器。
在Trap中指定力覺觸發物件並執行力觸發器功能。因此，可監控過度力。
在Till條件中指定力覺動作限制物件。
開始計算作業期間所施加的力的峰值。
執行力控制功能10秒。執行過程中，如果滿足為Till條件指定的力動作限制條件，則中斷動作。
讀取並顯示力覺感應器的目前值及峰值。在此範例中，僅會顯示數值。這些數值可用於判定操作通過或失敗，並進行分支處理。
讀取力運動限制的結果。在此範例中，如果滿足條件，則只會將錯誤顯示為溢出，但可以執行恢復過程等。
若在作業期間偵測到過度力，則會中斷程式並插入執行PressSample_EHandle函數。擷取並顯示操作期間應用的峰值，中止機器人動作，並輸入錯誤狀態。在此範例中，已輸入錯誤狀態，但也可在發生錯誤時進行重試等其他處理方式。

← 力運動限制功能的SPEL+程式教學 →