ID : 1181

Example Program of Force Sensor Robust Compliance Control

The following is an example program.

Description of the Example Program

This is an example program for a fitting operation.
Force Sensor Robust Compliance Control is started when the robot brings a workpiece to the start position of the force control.
The force sensor value is reset (0N, 0Nm) when the force control starts.
Force Sensor Robust Compliance Control ends when the robot has moved the workpiece a predetermined distance.

Example

Sub Main
'!TITLE "Fitting operation"
'
' Position to get workpiece		P[1]
' Force control starting position		P[2] 
' Once this travel distance is achieved, force control ends	F[1]

  Dim startPos As Position
  Dim rotStartPos As Position
  Dim snsrVal As Position
	
  TakeArm Keep = 0
  
  Move P, @e P[1]				'Position to get workpiece
  
  Delay 100
  
  hand0.Chuck 1					'Hand chuck
  
  Delay 100
  
  Move P, @e P[2]				'Starting position
	
  startPos = CurPos				'Set the current position as a starting position
  
  ForceSensor 0					'Reset sensor
  
  Delay 500
	
  ForceCtrl True, 1, Mode = 3			'Start Force Sensor Robust Compliance Control
	
  Do
  
  snsrVal = ForceValue(0)			'Acquire the sensor value
  
  'Condition for motion end
  'Wait until the tool end moves F1 mm from the force control starting position
        				
  If (Abs(PosZ(*)-PosZ(startPos)) > F[1]) Then GoTo StopProc 
  
  Loop
	
  StopProc:
  		
  ForceCtrl False				'End the force control
  
  hand0.Unchuck 0				'Hand unchuck
  
End Sub

ID : 1181

<< Prev Next >>