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
ledend: 1295
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
# ip: 192.168.1.209
# ledstart: 1296

2
jukebox-web

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

69
led_control.py
View File

@ -33,6 +33,7 @@ class LEDSystem():
changecount = 0
animation_time = 0
start = uptime()
solid_controllers = None
def __init__(self):
self.start = uptime()
@ -67,6 +68,7 @@ class LEDSystem():
self.leds = list()
self.leds_size = list()
self.controllers = list()
self.solid_controllers = list()
self.rings = list(range(len(self.config["position_map"])))
print("Setting ring status")
self.ringstatus = list(range(len(self.config["position_map"])))
@ -157,14 +159,17 @@ class LEDSystem():
# controller mapping
for ctrl in self.config["led"]["controllers"]:
if len(self.controllers) < ctrl["universe"]:
print(ctrl["universe"])
for x in range(ctrl["universe"] - len(self.controllers)):
self.controllers.append(None)
if ctrl["mode"] == "solid":
self.solid_controllers.append((ctrl["universe"]-1, ctrl["color"], ctrl["ip"]))
else:
if len(self.controllers) < ctrl["universe"]:
print(ctrl["universe"])
for x in range(ctrl["universe"] - len(self.controllers)):
self.controllers.append(None)
self.controllers[ctrl["universe"]-1] = (ctrl["ledstart"],ctrl["ledend"]+1,ctrl["ip"])
for x in range(ctrl["ledstart"],ctrl["ledend"]+1):
self.leds_size[x] = len(ctrl["mode"])
self.controllers[ctrl["universe"]-1] = (ctrl["ledstart"],ctrl["ledend"]+1,ctrl["ip"])
for x in range(ctrl["ledstart"],ctrl["ledend"]+1):
self.leds_size[x] = len(ctrl["mode"])
#fprint(controllers)
if(self.debug):
@ -200,11 +205,29 @@ class LEDSystem():
print(" done")
#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)
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.")
self.sender.activate_output(x+1) # start sending out data
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
# initialize global pixel data list
@ -213,7 +236,7 @@ class LEDSystem():
for x in range(len(self.leds)):
if self.leds_size[x] == 3:
self.exactdata.append(None)
self.data.append((20,20,127))
self.data.append((0,0,127))
elif self.leds_size[x] == 4:
self.exactdata.append(None)
self.data.append((50,50,255,0))
@ -238,6 +261,12 @@ class LEDSystem():
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
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()
time.sleep(0.002)
#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)
else:
self.setring(0,100,0,ring)
self.setmode("Moving")
self.setmode("Idle")
elif self.mode == "GrabC":
if self.firstrun:
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)
else:
self.setring(0,50,100,ring)
self.setmode("Moving")
self.setmode("Idle")
elif self.mode == "GrabAA":
print("GrabAA")
if self.firstrun:
self.firstrun = False
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)
else:
self.setring(100,0,0,ring)
self.setmode("Moving")
self.setmode("Idle")
elif self.mode == "GrabAB":
if self.firstrun:
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)
else:
self.setring(0,100,0,ring)
self.setmode("Moving")
self.setmode("Idle")
elif self.mode == "Moving":
elif self.mode == "Idle":
if self.firstrun:
self.firstrun = False
if arm_position is not None:
@ -448,11 +478,8 @@ class LEDSystem():
#if ratio < 0:
# ratio = 0
self.data[idx] = (int(base[0] + ratio * deltar), int(base[1] + ratio * deltag), 100) #base[2] + ratio * deltab)
elif self.mode == "idle":
self.mode = "Moving"
time.sleep(0)
else:
print("No arm position!")
self.sendall(self.data)
return self
@ -692,7 +719,7 @@ class LEDSystem():
for x in range(self.animation_time):
self.mainloop(None, preview=show)
self.clear_animations()
stmode = "idle"
stmode = "Idle"
self.mainloop(stmode, preview=show)
self.clear_animations()
return self
@ -712,7 +739,7 @@ class LEDSystem():
return self
def wait_for_animation(self, ring=-1):
while self.mode != "idle":
while self.mode != "Idle":
self.mainloop(None, ring, preview=show)
return self
@ -726,7 +753,7 @@ if __name__ == "__main__":
ret, frame = cap.read()
if not ret:
break
ledsys.mapimage(frame, fps=200)
ledsys.mapimage(frame, fps=60)
show = False
ring = 1

107
run.py
View File

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

36
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,7 +520,9 @@ 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()
@ -532,7 +533,8 @@ def holder_routine(robot, pos_updates, holder_index, pick_up, verbose=False):
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)