HEADER_PROG = ROBOTIQ GRIPPER UTILITY
  def RQ_2FG_Close():
    global Position_close=255
    global Speed_close=255
    global Force_close=255
    #Allocation of modbus output registers with the desired settings
    out_reg2_close = Speed_close * 256 + Force_close
    modbus_set_output_register("Speed_Force_Req", out_reg2_close, False)
    modbus_set_output_register("Position_Req", Position_close, False)
    
    #Allow movement
    modbus_set_output_signal("Go_to_Requested", True, False)
    while(modbus_get_signal_status("Action_Status", False) == False):
      sync()
    end
    
    #Wait until the end of the movement
    sleep(0.2)
    while (not((modbus_get_signal_status("Obj_Det_Status1", False) == True) or (modbus_get_signal_status("Obj_Det_Status2", False) == True ))):
      sleep(0.1)
      sync()
    end
    
    sleep(0.2)
    #Status check object detection
    global obj_detect1 = modbus_get_signal_status("Obj_Det_Status1",False)
    global obj_detect2 = modbus_get_signal_status("Obj_Det_Status2",False)
    if ((obj_detect1 == False) and (obj_detect2 == True)):
      global Contact_opening = False
      global Contact_closing = True
    else:
      if ((obj_detect1 == True) and (obj_detect2 == False)):
        global Contact_opening = True
        global Contact_closing = False
      else:
        global Contact_opening = False
        global Contact_closing = False
      end
    end
    
    #Prohibit the movement
    modbus_set_output_signal("Go_to_Requested", False, False)
    while(modbus_get_signal_status("Action_Status", False) == True):
      sync()
    end
  end
  def RQ_2FG_Open():
    global Position_open=0
    global Speed_open=255
    global Force_open=255
    #Allocation of modbus output registers with the desired settings
    out_reg2_open = Speed_open * 256 + Force_open
    modbus_set_output_register("Speed_Force_Req", out_reg2_open, False)
    modbus_set_output_register("Position_Req", Position_open, False)
    
    #Allow movement
    modbus_set_output_signal("Go_to_Requested", True, False)
    while(modbus_get_signal_status("Action_Status", False) == False):
      sync()
    end
    
    #Wait until the end of the movement
    sleep(0.2)
    while (not((modbus_get_signal_status("Obj_Det_Status1", False) == True) or (modbus_get_signal_status("Obj_Det_Status2", False) == True ))):
      sleep(0.1)
      sync()
    end
    
    sleep(0.2)
    #Status check object detection
    global obj_detect1 = modbus_get_signal_status("Obj_Det_Status1",False)
    global obj_detect2 = modbus_get_signal_status("Obj_Det_Status2",False)
    if ((obj_detect1 == False) and (obj_detect2 == True)):
      global Contact_opening = False
      global Contact_closing = True
    else:
      if ((obj_detect1 == True) and (obj_detect2 == False)):
        global Contact_opening = True
        global Contact_closing = False
      else:
        global Contact_opening = False
        global Contact_closing = False
      end
    end
    
    #Prohibit the movement
    modbus_set_output_signal("Go_to_Requested", False, False)
    while(modbus_get_signal_status("Action_Status", False) == True):
      sync()
    end
  end
  
  #Activation if not activated
  if ((modbus_get_signal_status("Gripper_Status1", False) == False)  and (modbus_get_signal_status("Gripper_Status2", False) == False)):
    modbus_set_output_signal("Activate", True, False)
  end
  
  #Prohibit the movement
  modbus_set_output_signal("Go_to_Requested", False, False)
  while (modbus_get_signal_status("Action_Status", False) == True):
    sync()
  end
  
  #Awaiting the activation of the gripper
  while (not(modbus_get_signal_status("Gripper_Status1", False) ==  True )):
    sync()
  end
  while (not(modbus_get_signal_status("Gripper_Status2", False) ==  True )):
    sync()
  end



def myprog():
  set_tcp(p[0.000, 0.000, 0.170, 0.000, 0.000, 0.000])
  movej([0, -1.5708, -1.5708, -3.1416, -1.5708, -1.5708],a=0.05,v=0.873,r=0.01)
  RQ_2FG_Open()
  movej(p[0.2752, -0.2294, 0.168,3.1357,0.1918,0],a=0.174,v=0.873,r=0.01)
  movel(p[0.2752, -0.2294, 0.168,3.1357,0.1918,0],a=0.1,v=0.1,r=0.01)
  movel(p[0.2752, -0.2294, 0.018,3.1357,0.1918,0],a=0.1,v=0.1,r=0.01)
  RQ_2FG_Close()
  sleep(3)
  movel(p[0.2752, -0.2294, 0.168,3.1357,0.1918,0],a=0.1,v=0.1,r=0.01)
end
myprog()