# 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
from robolink import *    # RoboDK API
from robodk import *      # Robot toolbox

RDK = Robolink()


# get the robot by name:
robot = RDK.Item('', ITEM_TYPE_ROBOT)

# Declare Variable

P=[
    [0,0,-500,180,0,0],
    [500,0,-500,180,0,90],
    [500,500,-500,180,0,0],
    [000,500,-500,180,0,90],
    [945,0,300,-180,0,0],
    [-200,1050,-500,-180,0,90]
   ]
Bag_Pos=0 # Giá tri cua cac bien vi tri bao bo.
Shift_Layer=0
Layer=0
# Notify user:
print('To edit this program:\nright click on the Python program, then, select "Edit Python script"')

# Program example:
item = RDK.Item('Motoman HP20D')
if item.Valid():
    print('Item selected: ' + item.Name())
    print('Item posistion: ' + repr(item.Pose()))

print('Items in the station:')
itemlist = RDK.ItemList()
print(itemlist)

#raise Exception('Program not edited.')


robot.setPoseFrame(RDK.Item('base'))
robot.setPoseTool(RDK.Item('Tool 0'))

robot.setPoseFrame(RDK.Item('USER 2'))
robot.setPoseTool(RDK.Item('Tool 0'))

for Layer in range(5):        
    for Bag_Pos in range (4):
        
        robot.setPoseFrame(RDK.Item('base'))#Set he toa do tham chieu là robot
        robot.setPoseTool(RDK.Item('Tool 0'))# Set su dung tool 0
        robot.MoveJ(xyzrpw_2_pose(P[4])) #Move ve diem trung gian

        P[5][2]+=200 # Day toa do Z len 1 khoang 200mm P5'
        robot.MoveJ(xyzrpw_2_pose(P[5])) # Di chuyen den vi tri P5'
        P[5][2]-=200 # Lay lai toa do P5
        robot.MoveL(xyzrpw_2_pose(P[5])) # Di chuyen den toa do P5
        robot.setPoseFrame(RDK.Item('base'))#Set he toa do tham chieu là robot
        P[5][2]+=200 # Day toa do Z len 1 khoang 200mm P5'
        robot.MoveL(xyzrpw_2_pose(P[5]))# Di chuyen den vi tri P5'
        P[5][2]-=200 # Lay lai toa do P5

        robot.MoveJ(xyzrpw_2_pose(P[4])) #Move ve diem trung gian
        
        robot.setPoseFrame(RDK.Item('USER 2')) #Set he toa do tham chieu là User coordinate1
        robot.setPoseFrame(robot.PoseFrame())
        robot.setPoseTool(RDK.Item('Tool 0'))# Set su dung tool 0

        Shift_Layer=Layer*120
        P[Bag_Pos][2]+=Shift_Layer
        
        P[Bag_Pos][2]+=200; # Day toa do Z len 1 khoang 200mm Pi'
        robot.MoveL(xyzrpw_2_pose(P[Bag_Pos]))# Di chuyen den vi tri Pi'
        P[Bag_Pos][2]-=200;# Lay lai toa do Pi
        robot.MoveL(xyzrpw_2_pose(P[Bag_Pos]))
        P[Bag_Pos][2]+=200;
        robot.MoveL(xyzrpw_2_pose(P[Bag_Pos]))
        P[Bag_Pos][2]-=200;
        
        robot.setPoseFrame(RDK.Item('base'))
        robot.setPoseTool(RDK.Item('Tool 0'))
        robot.MoveJ(xyzrpw_2_pose(P[4]))