def ShortTest():
  # Global parameters:
  global speed_ms    = 0.250
  global speed_rads  = 0.750
  global accel_mss   = 1.200
  global accel_radss = 1.200
  global blend_radius_m = 0.001
  
  #--------------------------
  # Add any default subprograms here
  # For example, to drive a gripper as a program call:
  # def Gripper_Open():
  #   ...
  # end
  #
  # Example to drive a spray gun:
  def SprayOn(value):
    # use the value as an output:
    DO_SPRAY = 5
    if value == 0:
      set_standard_digital_out(DO_SPRAY, False)
    else:
      set_standard_digital_out(DO_SPRAY, True)
    end
  end

  # Example to synchronize 2
  def Synchronize():
    # Use the following digital output to signal the state of the robot:
    DO_SYNC = 1
    
    # Use the following digital input to get the state of another robot:
    DI_SYNC = 1
    
    if (get_standard_digital_out(DO_SYNC) == get_standard_digital_in(DI_SYNC)):
      set_standard_digital_out(DO_SYNC, not (get_standard_digital_out(DI_SYNC)))
      sleep(0.1)
      thread Thread_wait_1():
        while (True):
          sleep(0.01)
        end
      end
      if (get_standard_digital_out(DO_SYNC) != get_standard_digital_in(DI_SYNC)):
        global thread_handler_1=run Thread_wait_1()
        while (get_standard_digital_out(DO_SYNC) != get_standard_digital_in(DI_SYNC)):
          sync()
        end
        kill thread_handler_1
      end
    else:
      if (get_standard_digital_out(DO_SYNC) != get_standard_digital_in(DI_SYNC)):
        set_standard_digital_out(DO_SYNC, not (get_standard_digital_out(DO_SYNC)))
      end
    end
  end

  #
  # Example to move an external axis
  def MoveAxis(value):
    # use the value as an output:
    DO_AXIS_1 = 1
    DI_AXIS_1 = 1
    if value <= 0:
      set_standard_digital_out(DO_AXIS_1, False)
      
      # Wait for digital input to change state
      #while (get_standard_digital_in(DI_AXIS_1) != False):
      #  sync()
      #end
    else:
      set_standard_digital_out(DO_AXIS_1, True)
      
      # Wait for digital input to change state
      #while (get_standard_digital_in(DI_AXIS_1) != True):
      #  sync()
      #end
    end
  end
  #--------------------------
  
  
  # Main program:
  # Program generated by RoboDK v4.2.4 for UR5 on 19/05/2020 11:12:05
  # Using nominal kinematics.
  # Using Ref. Frame 2: p[-0.564302, -0.032196, 0.181007, 0.000000, 0.000000, 0.000000]
  # set_reference(p[-0.564302, -0.032196, 0.181007, 0.000000, 0.000000, 0.000000])
  # Using TCP Gripper RobotiQ 85 Opened: p[0.000000, 0.000000, 0.130000, 0.000000, 0.000000, 0.000000]
  set_tcp(p[0.000000, 0.000000, 0.130000, 0.000000, 0.000000, 0.000000])
  movej([-2.825642, -1.818664, -1.016801, -1.857682, 1.555550, 1.331379],1.20000,0.25000,0.0000)
  movej([-2.692058, -1.744109, -1.121994, -1.829246, 1.553123, 1.464958],1.20000,0.25000,0.0010)
  # End of main program
end

ShortTest()