# Type help("robolink") or help("robodk") for more information
# Press F5 to run the script
# Documentation: https://robodk.com/doc/en/RoboDK-API.html
# Reference:     https://robodk.com/doc/en/PythonAPI/index.html
# Note: It is not required to keep a copy of this file, your python script is saved with the station

import time

from robolink import *              # import the robolink library (bridge with RoboDK)
RDK = Robolink()                    # establish a link with the simulator
robot = RDK.Item('Omron TM5-900')      # retrieve the robot by name
gripper = RDK.Item('RobotiQ HandE Gripper')
#robot.setJoints([-130,0,110,0,0,0])      # set all robot axes to zero

#target = RDK.Item('Target 1')         # retrieve the Target item
#robot.MoveJ(target)                 # move the robot to the target

# calculate a new approach position 100 mm along the Z axis of the tool with respect to the target
from robodk import *                # import the robodk library (robotics toolbox)

##### Gripper movement :)
for x in range(3):
	gripper.RunInstruction('RQ_2FG_Close', INSTRUCTION_CALL_PROGRAM)
	time.sleep(1)
	gripper.RunInstruction('RQ_2FG_Open', INSTRUCTION_CALL_PROGRAM)
	time.sleep(1)




##### Robot movement :)
# robot.setRounding(7)

# # Run 2 turn program
# robot.MoveL(target.Pose()*transl(0,0,0))
# robot.MoveL(target.Pose()*transl(50,0,0))
# robot.MoveL(target.Pose()*transl(50,50,0))
# robot.MoveL(target.Pose()*transl(0,50,0))

# # Run 4 turn program
# y = 75
# robot.MoveL(target.Pose()*transl(0,0+y,0))
# robot.MoveL(target.Pose()*transl(37.5,0+y,0))
# robot.MoveL(target.Pose()*transl(50,12.5+y,0))
# robot.MoveL(target.Pose()*transl(50,37.5+y,0))
# robot.MoveL(target.Pose()*transl(37.5,50+y,0))
# robot.MoveL(target.Pose()*transl(0,50+y,0))


# # Run 8 turn program
# y = 150
# robot.MoveL(target.Pose()*transl(0,0+y,0))
# robot.MoveL(target.Pose()*transl(22.7,0+y,0))
# robot.MoveL(target.Pose()*transl(39.9,7.14+y,0))
# robot.MoveL(target.Pose()*transl(47,14.28+y,0))
# robot.MoveL(target.Pose()*transl(50,21.42+y,0))
# robot.MoveL(target.Pose()*transl(50,28.6+y,0))
# robot.MoveL(target.Pose()*transl(47,35.7+y,0))
# robot.MoveL(target.Pose()*transl(39.9,42.8+y,0))
# robot.MoveL(target.Pose()*transl(22.7,50+y,0))
# robot.MoveL(target.Pose()*transl(0,50+y,0))

# # Back to original position
# robot.MoveL(target.Pose()*transl(-100,200,0))