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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
config.yml
View File

@ -78,6 +78,18 @@ led:
ledstart: 1152 ledstart: 1152
ledend: 1295 ledend: 1295
mode: rgb mode: rgb
- universe: 10
ip: 192.168.1.209
mode: solid
color: [0, 50, 150]
- universe: 11
ip: 192.168.1.210
mode: solid
color: [0, 50, 150]
- universe: 12
ip: 192.168.1.211
mode: solid
color: [0, 50, 150]
# - universe: 0 # - universe: 0
# ip: 192.168.1.209 # ip: 192.168.1.209
# ledstart: 1296 # ledstart: 1296

2
jukebox-web

@ -1 +1 @@
Subproject commit 8824070547e7d183ea3ede4d7040f23aa36c2c2a Subproject commit 8fe33095ddd16d1b975453b0ee0e7a52767605f8

55
led_control.py
View File

@ -33,6 +33,7 @@ class LEDSystem():
changecount = 0 changecount = 0
animation_time = 0 animation_time = 0
start = uptime() start = uptime()
solid_controllers = None
def __init__(self): def __init__(self):
self.start = uptime() self.start = uptime()
@ -67,6 +68,7 @@ class LEDSystem():
self.leds = list() self.leds = list()
self.leds_size = list() self.leds_size = list()
self.controllers = list() self.controllers = list()
self.solid_controllers = list()
self.rings = list(range(len(self.config["position_map"]))) self.rings = list(range(len(self.config["position_map"])))
print("Setting ring status") print("Setting ring status")
self.ringstatus = list(range(len(self.config["position_map"]))) self.ringstatus = list(range(len(self.config["position_map"])))
@ -157,6 +159,9 @@ class LEDSystem():
# controller mapping # controller mapping
for ctrl in self.config["led"]["controllers"]: for ctrl in self.config["led"]["controllers"]:
if ctrl["mode"] == "solid":
self.solid_controllers.append((ctrl["universe"]-1, ctrl["color"], ctrl["ip"]))
else:
if len(self.controllers) < ctrl["universe"]: if len(self.controllers) < ctrl["universe"]:
print(ctrl["universe"]) print(ctrl["universe"])
for x in range(ctrl["universe"] - len(self.controllers)): for x in range(ctrl["universe"] - len(self.controllers)):
@ -200,11 +205,29 @@ class LEDSystem():
print(" done") print(" done")
#if count < self.config["led"]["timeout"]: #if count < self.config["led"]["timeout"]:
for x in range(len(self.solid_controllers)):
print("Waiting for the controller at", self.solid_controllers[x][2], "to be online...", end="", flush=True)
count = 0
while not self.ping(self.solid_controllers[x][2]):
count = count + 1
if count >= self.config["led"]["timeout"]:
print(" ERROR: controller still offline after " + str(count) + " seconds, continuing...")
break
else:
print(" done")
time.sleep(1) time.sleep(1)
for x in range(len(self.controllers)): for x in range(len(self.controllers)):
print("Activating controller", x+1, "at", self.controllers[x][2], "with", self.controllers[x][1]-self.controllers[x][0], "LEDs.") print("Activating controller", x+1, "at", self.controllers[x][2], "with", self.controllers[x][1]-self.controllers[x][0], "LEDs.")
self.sender.activate_output(x+1) # start sending out data self.sender.activate_output(x+1) # start sending out data
self.sender[x+1].destination = self.controllers[x][2] self.sender[x+1].destination = self.controllers[x][2]
for x in range(len(self.solid_controllers)):
ctrl = self.solid_controllers[x]
print("Activating controller", ctrl[0], "at", ctrl[2], "with solid color ", ctrl[1])
self.sender.activate_output(ctrl[0]+1) # start sending out data
self.sender[ctrl[0]+1].destination = ctrl[2]
self.sender.manual_flush = True self.sender.manual_flush = True
# initialize global pixel data list # initialize global pixel data list
@ -213,7 +236,7 @@ class LEDSystem():
for x in range(len(self.leds)): for x in range(len(self.leds)):
if self.leds_size[x] == 3: if self.leds_size[x] == 3:
self.exactdata.append(None) self.exactdata.append(None)
self.data.append((20,20,127)) self.data.append((0,0,127))
elif self.leds_size[x] == 4: elif self.leds_size[x] == 4:
self.exactdata.append(None) self.exactdata.append(None)
self.data.append((50,50,255,0)) self.data.append((50,50,255,0))
@ -238,6 +261,12 @@ class LEDSystem():
for x in range(len(self.controllers)): for x in range(len(self.controllers)):
self.sender[x+1].dmx_data = list(sum(datain[self.controllers[x][0]:self.controllers[x][1]] , ())) # flatten the subsection of the data array self.sender[x+1].dmx_data = list(sum(datain[self.controllers[x][0]:self.controllers[x][1]] , ())) # flatten the subsection of the data array
offset = len(self.controllers)
for x in range(len(self.solid_controllers)):
ctrl = self.solid_controllers[x]
self.sender[ctrl[0]+1].dmx_data = list(ctrl[1]) * 170
self.sender.flush() self.sender.flush()
time.sleep(0.002) time.sleep(0.002)
#sender.flush() # 100% reliable with 2 flushes, often fails with 1 #sender.flush() # 100% reliable with 2 flushes, often fails with 1
@ -391,7 +420,7 @@ class LEDSystem():
self.changecount = self.fadeorder(self.rings[ring][2],self.rings[ring][2]+24, self.changecount, 0,100,0) self.changecount = self.fadeorder(self.rings[ring][2],self.rings[ring][2]+24, self.changecount, 0,100,0)
else: else:
self.setring(0,100,0,ring) self.setring(0,100,0,ring)
self.setmode("Moving") self.setmode("Idle")
elif self.mode == "GrabC": elif self.mode == "GrabC":
if self.firstrun: if self.firstrun:
self.firstrun = False self.firstrun = False
@ -400,9 +429,10 @@ class LEDSystem():
self.changecount = self.fadeall(self.rings[ring][2],self.rings[ring][3], self.changecount, 0,50,100) self.changecount = self.fadeall(self.rings[ring][2],self.rings[ring][3], self.changecount, 0,50,100)
else: else:
self.setring(0,50,100,ring) self.setring(0,50,100,ring)
self.setmode("Moving") self.setmode("Idle")
elif self.mode == "GrabAA": elif self.mode == "GrabAA":
print("GrabAA")
if self.firstrun: if self.firstrun:
self.firstrun = False self.firstrun = False
self.changecount = self.animation_time # 100hz self.changecount = self.animation_time # 100hz
@ -410,7 +440,7 @@ class LEDSystem():
self.changecount = self.fadeall(self.rings[ring][2],self.rings[ring][3], self.changecount, 100,0,0) self.changecount = self.fadeall(self.rings[ring][2],self.rings[ring][3], self.changecount, 100,0,0)
else: else:
self.setring(100,0,0,ring) self.setring(100,0,0,ring)
self.setmode("Moving") self.setmode("Idle")
elif self.mode == "GrabAB": elif self.mode == "GrabAB":
if self.firstrun: if self.firstrun:
self.firstrun = False self.firstrun = False
@ -420,9 +450,9 @@ class LEDSystem():
self.changecount = self.fadeorder(self.rings[ring][2],self.rings[ring][2]+24, self.changecount, 0,100,0) self.changecount = self.fadeorder(self.rings[ring][2],self.rings[ring][2]+24, self.changecount, 0,100,0)
else: else:
self.setring(0,100,0,ring) self.setring(0,100,0,ring)
self.setmode("Moving") self.setmode("Idle")
elif self.mode == "Moving": elif self.mode == "Idle":
if self.firstrun: if self.firstrun:
self.firstrun = False self.firstrun = False
if arm_position is not None: if arm_position is not None:
@ -448,11 +478,8 @@ class LEDSystem():
#if ratio < 0: #if ratio < 0:
# ratio = 0 # ratio = 0
self.data[idx] = (int(base[0] + ratio * deltar), int(base[1] + ratio * deltag), 100) #base[2] + ratio * deltab) self.data[idx] = (int(base[0] + ratio * deltar), int(base[1] + ratio * deltag), 100) #base[2] + ratio * deltab)
else:
elif self.mode == "idle": print("No arm position!")
self.mode = "Moving"
time.sleep(0)
self.sendall(self.data) self.sendall(self.data)
return self return self
@ -692,7 +719,7 @@ class LEDSystem():
for x in range(self.animation_time): for x in range(self.animation_time):
self.mainloop(None, preview=show) self.mainloop(None, preview=show)
self.clear_animations() self.clear_animations()
stmode = "idle" stmode = "Idle"
self.mainloop(stmode, preview=show) self.mainloop(stmode, preview=show)
self.clear_animations() self.clear_animations()
return self return self
@ -712,7 +739,7 @@ class LEDSystem():
return self return self
def wait_for_animation(self, ring=-1): def wait_for_animation(self, ring=-1):
while self.mode != "idle": while self.mode != "Idle":
self.mainloop(None, ring, preview=show) self.mainloop(None, ring, preview=show)
return self return self
@ -726,7 +753,7 @@ if __name__ == "__main__":
ret, frame = cap.read() ret, frame = cap.read()
if not ret: if not ret:
break break
ledsys.mapimage(frame, fps=200) ledsys.mapimage(frame, fps=60)
show = False show = False
ring = 1 ring = 1

93
run.py
View File

@ -32,7 +32,7 @@ from uptime import uptime
import fileserver import fileserver
# set to false to run without real hardware for development # set to false to run without real hardware for development
real = False real = True
skip_scanning = True skip_scanning = True
mbconn = None mbconn = None
@ -71,6 +71,8 @@ arm_updates = None
animation_wait = False animation_wait = False
arm_position = (0,0,0,0,0,0) arm_position = (0,0,0,0,0,0)
arm_position_process = None arm_position_process = None
start_animation = False
failcount = 0
def arm_start_callback(res): def arm_start_callback(res):
fprint("Arm action complete.") fprint("Arm action complete.")
@ -531,6 +533,8 @@ def mainloop_server(pool, manager):
global animation_wait global animation_wait
global arm_position global arm_position
global arm_position_process global arm_position_process
global start_animation
global failcount
if mode != oldmode: if mode != oldmode:
print(" ***** Running mode:", mode, "***** ") print(" ***** Running mode:", mode, "***** ")
@ -555,23 +559,64 @@ def mainloop_server(pool, manager):
print("Arm queue message " + str(val)) print("Arm queue message " + str(val))
checkpoint = val checkpoint = val
print(ring_animation, animation_wait, ledsys.mode, arm_position) print(ring_animation, animation_wait, ledsys.mode, arm_position)
if ring_animation is not None and ledsys.mode != "idle" and real:
ledsys.mainloop(None, ring_animation, arm_position=arm_position)
elif ring_animation is not None and real:
if start_animation:
# animation start requested
# may not be immediate
if ring_animation is not None:
if animation_wait: if animation_wait:
if checkpoint is not None: # got to checkpoint from UR5 # wait for checkpoint
fprint("Starting checkpointed animation " + led_set_mode + " for ring " + str(ring_animation)) if checkpoint is not None:
fprint("Starting checkpointed animation " + str(led_set_mode) + " for ring " + str(ring_animation))
ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position) ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
led_set_mode = None led_set_mode = None
animation_wait = False animation_wait = False
start_animation = False
else: else:
fprint("Starting immediate animation " + led_set_mode + " for ring " + str(ring_animation)) # still waiting
ledsys.mainloop(None, ring_animation, arm_position=arm_position)
else:
# no waiting, just start
fprint("Starting immediate animation " + str(led_set_mode) + " for ring " + str(ring_animation))
ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position) ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
led_set_mode = None led_set_mode = None
animation_wait = False
start_animation = False
else:
# no ring animation specified
pass
else:
# no new animation
if ring_animation is not None:
ledsys.mainloop(None, ring_animation, arm_position=arm_position)
else: else:
pass pass
#fprint("Not triggering LED loop: no ring animation")
# if start_animation is False and ring_animation is not None and ledsys.mode != "Idle" and real:
# ledsys.mainloop(None, ring_animation, arm_position=arm_position)
# elif start_animation is True and ring_animation is not None and real:
# if animation_wait:
# if checkpoint is not None: # got to checkpoint from UR5
# fprint("Starting checkpointed animation " + str(led_set_mode) + " for ring " + str(ring_animation))
# ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
# led_set_mode = None
# animation_wait = False
# start_animation = False
# else:
# fprint("Starting immediate animation " + str(led_set_mode) + " for ring " + str(ring_animation))
# ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
# led_set_mode = None
# start_animation = False
# else:
# ledsys.mainloop(None, 49, arm_position=arm_position)
# pass
# #fprint("Not triggering LED loop: no ring animation")
if mode == "Startup": if mode == "Startup":
if not real or skip_scanning: if not real or skip_scanning:
@ -587,6 +632,7 @@ def mainloop_server(pool, manager):
arm_state = "GET" arm_state = "GET"
ring_animation = counter ring_animation = counter
animation_wait = True animation_wait = True
start_animation = True
led_set_mode = "GrabA" led_set_mode = "GrabA"
#ur5_control.to_camera(arm, counter) #ur5_control.to_camera(arm, counter)
#arm_ready = True #arm_ready = True
@ -599,6 +645,7 @@ def mainloop_server(pool, manager):
elif camera_ready: elif camera_ready:
ring_animation = counter ring_animation = counter
animation_wait = True animation_wait = True
start_animation = True
led_set_mode = "GrabC" led_set_mode = "GrabC"
fprint("Adding cable to list...") fprint("Adding cable to list...")
global scan_value global scan_value
@ -624,7 +671,8 @@ def mainloop_server(pool, manager):
else: else:
# scanned everything # scanned everything
ring_animation = None ring_animation = None
led_set_mode == "idle" led_set_mode == "Idle"
start_animation = True
tmp = [ tmp = [
# Actual cables in Jukebox # Actual cables in Jukebox
"BLTF-1LF-006-RS5", "BLTF-1LF-006-RS5",
@ -735,6 +783,7 @@ def mainloop_server(pool, manager):
if real: if real:
animation_wait = False animation_wait = False
ring_animation = get_cable ring_animation = get_cable
start_animation = True
led_set_mode = "GrabAA" led_set_mode = "GrabAA"
pool.apply_async(ur5_control.holder_to_tray, (arm, arm_updates, get_cable, spot), callback=arm_start_callback, error_callback=handle_error) pool.apply_async(ur5_control.holder_to_tray, (arm, arm_updates, get_cable, spot), callback=arm_start_callback, error_callback=handle_error)
else: else:
@ -748,6 +797,7 @@ def mainloop_server(pool, manager):
arm_ready = False arm_ready = False
fprint("Returning cable from tray position " + str(get_cable)) fprint("Returning cable from tray position " + str(get_cable))
if real: if real:
failcount = 0
pool.apply_async(ur5_control.tray_to_camera, (arm, arm_updates, get_cable), callback=arm_start_callback, error_callback=handle_error) pool.apply_async(ur5_control.tray_to_camera, (arm, arm_updates, get_cable), callback=arm_start_callback, error_callback=handle_error)
else: else:
arm_ready = True arm_ready = True
@ -764,8 +814,9 @@ def mainloop_server(pool, manager):
else: else:
# getting cable and bringing to tray # getting cable and bringing to tray
# led animation # led animation
if ledsys.mode == "idle": if ledsys.mode == "Idle" and led_set_mode != "GrabAA":
animation_wait = True animation_wait = True
start_animation = True
led_set_mode = "GrabAB" led_set_mode = "GrabAB"
pass pass
@ -777,9 +828,10 @@ def mainloop_server(pool, manager):
camera_ready = False camera_ready = False
if real: if real:
animation_wait = False animation_wait = False
start_animation = True
ring_animation = 49 ring_animation = 49
led_set_mode = "Camera" led_set_mode = "Camera"
pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error) pool.apply_async(camera.read_qr, (50,), callback=camera_start_callback, error_callback=handle_error)
else: else:
camera_ready = True camera_ready = True
scan_value = "10GXS13" scan_value = "10GXS13"
@ -793,10 +845,18 @@ def mainloop_server(pool, manager):
if camera_ready == True: if camera_ready == True:
if scan_value is False: if scan_value is False:
# unable to scan ???? not good # unable to scan ???? not good
if failcount > 15:
mode = "Idle"
fprint("Giving up scanning cable.")
failcount = 0
else:
fprint("Unable to scan cable. Gonna retry.") fprint("Unable to scan cable. Gonna retry.")
camera_ready = False camera_ready = False
#mode = "Idle"
failcount += 1
pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error) pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
pass
elif scan_value.find("bldn.app/") > -1: elif scan_value.find("bldn.app/") > -1:
scan_value = scan_value[scan_value.find("bldn.app/")+9:] scan_value = scan_value[scan_value.find("bldn.app/")+9:]
@ -812,7 +872,8 @@ def mainloop_server(pool, manager):
animation_wait = True animation_wait = True
ring_animation = idx ring_animation = idx
led_set_mode = "GrabC" led_set_mode = "GrabC"
pool.apply_async(ur5_control.camera_to_holder, (arm, idx), callback=arm_start_callback, error_callback=handle_error) start_animation = True
pool.apply_async(ur5_control.camera_to_holder, (arm, arm_updates, idx), callback=arm_start_callback, error_callback=handle_error)
else: else:
arm_ready = True arm_ready = True
mode = "Return" mode = "Return"
@ -824,12 +885,14 @@ def mainloop_server(pool, manager):
animation_wait = True animation_wait = True
ring_animation = idx ring_animation = idx
led_set_mode = "GrabC" led_set_mode = "GrabC"
pool.apply_async(ur5_control.camera_to_holder, (arm, idx), callback=arm_start_callback, error_callback=handle_error) start_animation = True
pool.apply_async(ur5_control.camera_to_holder, (arm, arm_updates, idx), callback=arm_start_callback, error_callback=handle_error)
else: else:
arm_ready = True arm_ready = True
mode = "Return" mode = "Return"
break break
if mode == "Scan":
mode = "Idle"

32
ur5_control.py
View File

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