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

prerelease

Browse Source
This commit is contained in:
Cole Deck
2024-05-13 02:21:07 -05:00
parent fab8324dea
commit 5e0f1e93d2
5 changed files with 168 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
ur5_control.py
View File

@@ -80,7 +80,7 @@ def connect(robot):
while trying and count < 10:
count += 1
try:
robot.robot = urx.Robot(ip, use_rt=True)
robot.robot = urx.Robot(ip, use_rt=False)
robot.robot.set_tcp((robot.offset_x, robot.offset_y, robot.offset_z, 0, 0, 0))
# Set weight
robot.robot.set_payload(2, (0, 0, 0.1))
@@ -511,8 +511,7 @@ def holder_routine(robot, pos_updates, 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)
pos_updates.put(1)
fprint("Triggering LED interface")
if pick_up:
open_gripper()
@@ -521,18 +520,21 @@ def holder_routine(robot, pos_updates, holder_index, pick_up, verbose=False):
new_pos = curr_pos
new_pos[2] = 0.005
rob.movel(new_pos, vel=0.1, acc=1)
if pos_updates is not None:
pos_updates.put(1)
fprint("Triggering LED interface")
# Pick up or drop off
if pick_up:
close_gripper()
else:
open_gripper()
# Move up
new_pos[2] = 0.2
rob.movel(new_pos, vel=2, acc=1)
was_flipped = is_flipped(robot)
pos_updates.put(2)
if pos_updates is not None:
pos_updates.put(2)
fprint("Triggering LED interface")
# goto_holder_index(robot, 25, z=0.2)
def pick_up_holder(robot, pos_updates, holder_index, verbose=False):
@@ -681,7 +683,7 @@ def open_gripper():
c.write_single_register(112, 0b0)
c.write_single_register(435, 0b10000000)
time.sleep(0.5)
c.write_single_register(112, 0b0)
c.write_single_register(435, 0b10000000)
time.sleep(0.5)
@@ -694,7 +696,7 @@ def close_gripper():
c = ModbusClient(host="192.168.1.21", port=502, auto_open=True, auto_close=False)
c.write_single_register(435, 0b00000000)
c.write_single_register(112, 0b1)
time.sleep(0.5)
c.write_single_register(435, 0b00000000)
c.write_single_register(112, 0b1)
time.sleep(0.5)
@@ -704,7 +706,7 @@ def close_gripper():
def get_position_thread(robot, pos_updates):
robot = connect(robot)
rob = robot.robot
oldvals = rob.getl_rt()
oldvals = rob.getl()
deltavals = [0,0,0]
import uptime
t = 0.01
@@ -712,9 +714,10 @@ def get_position_thread(robot, pos_updates):
while True:
start = uptime.uptime()
if pos_updates.qsize() < 2:
vals = rob.getl_rt()
vals = rob.getl()
if vals != oldvals:
pos_updates.put(tuple(oldvals))
if pos_updates is not None:
pos_updates.put(tuple(oldvals))
#time.sleep(0.01)
# deltavals = list()
# deltavals.append(vals[0]-oldvals[0])
@@ -755,6 +758,9 @@ if __name__ == "__main__":
# pick_up_holder(robot, 2)
# drop_off_tray(robot, 0)
# drop_off_tray(robot, 1)
# drop_off_tray(robot, 2)
# drop_off_tray(robot, 3)
# pick_up_tray(robot, 1)
# drop_off_holder(robot, 5)
@@ -763,12 +769,12 @@ if __name__ == "__main__":
# drop_off_tray(robot, 3)
for i in range(44,45):
pick_up_holder(robot, i)
for i in range(0,54):
pick_up_holder(robot, None, i)
print('Drop off', i+1)
drop_off_holder(robot, i+1)
input()
#print('Drop off', i+1)
drop_off_tray(robot, 0)
#input()
# holder_to_camera(robot, 0)
# camera_to_holder(robot, 0)