from robolink import *    # API de RoboDK
from robodk import *      # Toolbox pour la robotique
from threading import Event #import pour la pause entre chaque étape

# Démarrer la API de RoboDK:
RDK = Robolink()

# Prendre le robot (premier robot trouvé):
robot = RDK.Item('', ITEM_TYPE_ROBOT)
robot.setSpeed(speed_linear=300, speed_joints= 100, accel_linear=- 1, accel_joints=- 1)
pince = RDK.Item('RobotiQ Hand-E Gripper')
# Recup targets:
Home = RDK.Item('Home_gruyere')
olive_over = RDK.Item('olive_over')
olive = RDK.Item('olive')
gruyere_init = RDK.Item('gruyere_init')
gruyere_over_init = RDK.Item('gruyere_over_init')
chat_start = RDK.Item('chat_start')
chat_before_start = RDK.Item('chat_before_start')
chat_end = RDK.Item('chat_end')
before_grab = RDK.Item('before_grab')
grab = RDK.Item('grab')
grab_remove = RDK.Item('grab_remove')
inter_chat_grab = RDK.Item('intermediaire_chat_grab')
inter_home_chat = RDK.Item('intermediaire_home_chat')


pause=0
target_pose = gruyere_init.Pose()
xyz_ref = target_pose.Pos()

target_pose_over = gruyere_over_init.Pose()
xyz_ref_over = target_pose_over.Pos()

target_pose_actual = gruyere_init.Pose()
target_pose_actual_over = gruyere_over_init.Pose()

robot.setPoseFrame(robot.PoseFrame())

print (xyz_ref[0],xyz_ref[1],xyz_ref[2])
# Bouger le robot à la cible:
for x in range (0,7):
    for y in range(0,6):
        
        target_pose_actual.setPos([xyz_ref[0]-(30*x),xyz_ref[1]+(20*y),xyz_ref[2]])
        target_pose_actual_over.setPos([xyz_ref[0]-(30*x),xyz_ref[1]+(20*y),xyz_ref_over[2]])
        #print('x =',x,'y=',y)
        #print (xyz_ref[0]-(20*x),xyz_ref[1]-(30*y),xyz_ref[2])
        #print(target_pose_actual)
        #print(target_pose_actual_over)
    
        robot.MoveJ(Home)
        Event().wait(pause)
        robot.MoveJ(inter_home_chat)
        Event().wait(pause) 
        robot.MoveJ(chat_before_start)
        Event().wait(pause) 
        robot.MoveJ(chat_start)
        Event().wait(pause) 
        robot.MoveJ(chat_end)
        Event().wait(pause)
        robot.MoveJ(inter_chat_grab)
        Event().wait(pause)  
        robot.MoveJ(before_grab)
        Event().wait(pause)
        robot.MoveJ(grab)
        Event().wait(pause)
        robot.MoveJ(grab_remove)
        Event().wait(pause) 
        robot.MoveJ(Home)
        Event().wait(pause) 
        robot.MoveJ(olive_over)
        Event().wait(pause) 
        robot.MoveJ(olive)
        Event().wait(pause)  
        robot.MoveJ(olive_over)
        Event().wait(pause) 
        robot.MoveJ(target_pose_actual_over)
        Event().wait(pause)  
        robot.MoveJ(target_pose_actual)
        Event().wait(pause) 
        robot.MoveJ(target_pose_actual_over)
        Event().wait(pause)  
        robot.MoveJ(Home)
        Event().wait(pause)