Jukebox/jukebox-software
10
0
Fork 0
Code Issues 3 Pull Requests Packages Projects Releases 2 Wiki Activity

Update LED system

Browse Source
This commit is contained in:
Cole Deck
2024-05-08 20:28:48 -05:00
parent dd7bc12fe5
commit f416e25c1c
4 changed files with 137 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
ur5_control.py
View File

@@ -21,7 +21,7 @@ class Rob():
#offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset
#
def __init__(self, config, pos_queue=Queue()):
def __init__(self, config):
self.config = config
armc = config["arm"]
self.ip = armc["ip"]
@@ -33,7 +33,7 @@ class Rob():
self.limb2 = limbs["limb2"]
self.limb3 = limbs["limb3"]
self.limb_wrist = limbs["limb_wrist"]
self.pos_updates = pos_queue
#self.init_arm()
def ping(host):
@@ -495,7 +495,7 @@ def safe_move(robot, x, y, z, use_closest_path=True):
rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot), use_closest_path=use_closest_path), vel=2, acc=2)
return True
def holder_routine(robot, holder_index, pick_up, verbose=False):
def holder_routine(robot, pos_updates, holder_index, pick_up, verbose=False):
robot = connect(robot)
rob = robot.robot
@@ -511,7 +511,8 @@ def holder_routine(robot, holder_index, pick_up, verbose=False):
goto_holder_index(robot, holder_index, 0.05, use_closest_path=False)
else:
goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
robot.pos_updates.put(1)
pos_updates.put(1)
fprint("Triggering LED interface")
if pick_up:
open_gripper()
@@ -531,12 +532,13 @@ def holder_routine(robot, holder_index, pick_up, verbose=False):
new_pos[2] = 0.2
rob.movel(new_pos, vel=2, acc=1)
was_flipped = is_flipped(robot)
robot.pos_updates.put(2)
pos_updates.put(2)
fprint("Triggering LED interface")
# goto_holder_index(robot, 25, z=0.2)
def pick_up_holder(robot, holder_index, verbose=False):
holder_routine(robot, holder_index, True, verbose=verbose)
def drop_off_holder(robot, holder_index, verbose=False):
holder_routine(robot, holder_index, False, verbose=verbose)
def pick_up_holder(robot, pos_updates, holder_index, verbose=False):
holder_routine(robot, pos_updates, holder_index, True, verbose=verbose)
def drop_off_holder(robot, pos_updates, holder_index, verbose=False):
holder_routine(robot, pos_updates, holder_index, False, verbose=verbose)
def tray_routine(robot, slot=0, pick_up=True):
robot = connect(robot)
@@ -648,29 +650,29 @@ def return_routine(robot, slot, holder_index=None, verbose=False):
return True
def goto_camera(robot):
def goto_camera(robot, pos_updates):
robot = connect(robot)
goto_holder_index(robot, 49, 0.2)
def tray_to_camera(robot, slot):
def tray_to_camera(robot, pos_updates, slot):
pick_up_tray(robot, slot)
goto_camera(robot)
def holder_to_tray(robot, holder_index, slot):
pick_up_holder(robot, holder_index)
goto_camera(robot, pos_updates)
def holder_to_tray(robot, pos_updates, holder_index, slot):
pick_up_holder(robot, pos_updates, holder_index)
drop_off_tray(robot, slot)
def holder_to_camera(robot, holder_index, verbose=False):
def holder_to_camera(robot, pos_updates, holder_index, verbose=False):
robot = connect(robot)
fprint("Bringing tube at " + str(holder_index) + " to camera")
rob = robot.robot
pick_up_holder(robot, holder_index)
goto_camera(robot)
def camera_to_holder(robot, holder_index, verbose=False):
pick_up_holder(robot, pos_updates, holder_index)
goto_camera(robot, pos_updates)
def camera_to_holder(robot, pos_updates, holder_index, verbose=False):
robot = connect(robot)
rob = robot.robot
drop_off_holder(robot, holder_index)
drop_off_holder(robot, pos_updates, holder_index)
def open_gripper():
@@ -699,15 +701,18 @@ def close_gripper():
c.close()
#
def get_position_thread(robot):
def get_position_thread(robot, pos_updates):
robot = connect(robot)
rob = robot.robot
vals = rob.getl()
robot.pos_updates.put(tuple(vals))
while True:
if pos_updates.qsize() < 2:
vals = rob.getl()
pos_updates.put(tuple(vals))
#print("Adding position to queue")
time.sleep(0.01)
if __name__ == "__main__":
#init("192.168.1.145")
with open('config.yml', 'r') as fileread:
#global config
config = yaml.safe_load(fileread)