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

5
config.yml
View File

@ -22,12 +22,13 @@ cables:
port: 7900 port: 7900
directory: ./cables/ # must include trailing slash directory: ./cables/ # must include trailing slash
#cable_map:
cameras: cameras:
banner: banner:
ip: 192.168.1.125 ip: 192.168.1.125
port: 32200 port: 32200
animation_time: 100
led: led:
fps: 100 fps: 100
timeout: 1 timeout: 1
@ -438,7 +439,7 @@ led:
# pos: [375, 300] # pos: [375, 300]
global_position_offset: [0,0] # default coordinate spce below as center of arm at 0,0 - adjust if necessary global_position_offset: [0,0] # default coordinate spce below as center of arm at 0,0 - adjust if necessary
animation_time: 200
position_map: position_map:
- index: 0 - index: 0
pos: [-152.4, 263.965] pos: [-152.4, 263.965]

69
led_control.py
View File

@ -315,12 +315,13 @@ class LEDSystem():
for x in range(start,end): for x in range(start,end):
self.data[x] = val self.data[x] = val
def setallrings(self, r,g,b, exclude): def setallringsexcept(self, r,g,b, exclude):
startidx1 = self.rings[0][2] startidx1 = self.rings[0][2]
endidx2 = self.rings[-1][3] endidx2 = self.rings[-1][3]
endidx1 = self.rings[exclude][2] endidx1 = self.rings[exclude][2]
startidx2 = self.rings[exclude][3]+1 startidx2 = self.rings[exclude][3]+1
self.setrange(startidx, endidx, r,g,b) self.setrange(startidx1, endidx1, r,g,b)
self.setrange(startidx2, endidx2, r,g,b)
def setring(self, r,g,b,idx): def setring(self, r,g,b,idx):
@ -336,7 +337,7 @@ class LEDSystem():
if self.mode == "Startup": if self.mode == "Startup":
# loading animation. cable check # loading animation. cable check
if self.firstrun: if self.firstrun:
self.changecount = self.animation_time * 3 self.changecount = self.animation_time * 2
self.firstrun = False self.firstrun = False
for x in range(len(self.ringstatus)): for x in range(len(self.ringstatus)):
self.ringstatus[x] = [True, self.animation_time] self.ringstatus[x] = [True, self.animation_time]
@ -357,7 +358,8 @@ class LEDSystem():
if self.ringstatus[x][0]: if self.ringstatus[x][0]:
self.setring(0, 50, 100, x) self.setring(0, 50, 100, x)
else: else:
self.ringstatus[x][1] = self.fadeall(self.rings[x][2],self.rings[x][3], self.ringstatus[x][1], 100,0,0) # not ready #self.ringstatus[x][1] = self.fadeall(self.rings[x][2],self.rings[x][3], self.ringstatus[x][1], 100,0,0) # not ready
self.ringstatus[x][1] = self.fadeall(self.rings[x][2],self.rings[x][3], self.ringstatus[x][1], 0,50,100)
elif self.mode == "StartupCheck": elif self.mode == "StartupCheck":
if self.firstrun: if self.firstrun:
@ -389,7 +391,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("idle") self.setmode("Moving")
elif self.mode == "GrabC": elif self.mode == "GrabC":
if self.firstrun: if self.firstrun:
self.firstrun = False self.firstrun = False
@ -398,28 +400,51 @@ 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")
elif self.mode == "GrabAA":
if self.firstrun:
self.firstrun = False
self.changecount = self.animation_time # 100hz
if self.changecount > 0:
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("idle") self.setmode("idle")
elif self.mode == "GrabAB":
if self.firstrun:
self.firstrun = False
self.changecount = self.animation_time # 100hz
if self.changecount > 0:
#self.changecount = self.fadeorder(self.rings[ring][2],self.rings[ring][3], self.changecount, 0,100,0)
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")
elif self.mode == "Moving": elif self.mode == "Moving":
if self.firstrun: if self.firstrun:
self.firstrun = False self.firstrun = False
posxy = arm_position[0:1] if arm_position is not None:
posxy[0] = int(posxy[0] * 1000) posxy = list()
posxy[0] = int(posxy[1] * 1000) posxy.append(int(arm_position[0] * 1000)) # x convert m to mm
radius = int(arm_position[2] * 1000) posxy.append(int(arm_position[1] * 1000)) # y
base = (0,50,100) radius = int(arm_position[2] * 1000)
target = (100,100,100) base = (0,50,100)
deltar = target[0] - base[0] target = (100,100,100)
deltag = target[0] - base[0] deltar = target[0] - base[0]
#deltab = target[0] - base[0] deltag = target[0] - base[0]
# reset! #deltab = target[0] - base[0]
self.setallringsexcept(0,50,100, ring) # reset!
# fade outwards self.setallringsexcept(0,50,100, ring)
for idx,led in enumerate(self.leds): exclude = self.rings[ring][2],self.rings[ring][3]
dist = int(math.srqt(math.pow(int(posxy[0] - led[0]), 2) + math.pow(int(posxy[1] - led[1]), 2))) # fade outwards
if dist < radius: for idx,led in enumerate(self.leds):
ratio = dist/radius if idx < exclude[0] or idx > exclude[1]:
self.data[idx] = (int(base[0] + ratio * deltar), int(base[1] + ratio * deltag), 100) #base[2] + ratio * deltab) dist = int(math.sqrt(math.pow(int(posxy[0] - led[0]), 2) + math.pow(int(posxy[1] - led[1]), 2)))
if dist < radius:
ratio = dist/radius
self.data[idx] = (int(base[0] + ratio * deltar), int(base[1] + ratio * deltag), 100) #base[2] + ratio * deltab)
elif self.mode == "idle": elif self.mode == "idle":
time.sleep(0) time.sleep(0)

88
run.py
View File

@ -67,9 +67,10 @@ ring_animation = None
led_set_mode = None led_set_mode = None
sensors = [0,0,0,0] sensors = [0,0,0,0]
websocket_process = None websocket_process = None
arm_updates = Queue() 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
def arm_start_callback(res): def arm_start_callback(res):
fprint("Arm action complete.") fprint("Arm action complete.")
@ -192,7 +193,7 @@ def check_server():
tmp1["application"] = key tmp1["application"] = key
else: else:
tmp1["application"] = fs["Product Overview"]["Suitable Applications:"] tmp1["application"] = fs["Product Overview"]["Suitable Applications:"]
if "image" in cabledata: if "image" in cabledata:
tmp1["image"] = cabledata["image"] tmp1["image"] = cabledata["image"]
if "description" in cabledata: if "description" in cabledata:
@ -332,7 +333,7 @@ def check_server_online(serverip, clientip):
return True return True
def setup_server(pool): def setup_server(pool, manager):
# linux server setup # linux server setup
global config global config
global counter global counter
@ -348,14 +349,15 @@ def setup_server(pool):
global from_server_queue global from_server_queue
global websocket_process global websocket_process
global arm_updates global arm_updates
global arm_position_process
global ledsys
arm = Rob(config, arm_updates) arm_updates = manager.Queue()
arm = Rob(config)
if real: if real:
pool.apply_async(ur5_control.powerup_arm, (arm,), callback=arm_start_callback, error_callback=handle_error) pool.apply_async(ur5_control.powerup_arm, (arm,), callback=arm_start_callback, error_callback=handle_error)
else: else:
arm_ready = True arm_ready = True
global ledsys
if real: if real:
ledsys = LEDSystem() ledsys = LEDSystem()
#pool.apply_async(ledsys.init, callback=led_start_callback) #pool.apply_async(ledsys.init, callback=led_start_callback)
@ -394,6 +396,9 @@ def setup_server(pool):
ur5_control.init_arm(arm) ur5_control.init_arm(arm)
fprint("Arm initialized.", sendqueue=to_server_queue) fprint("Arm initialized.", sendqueue=to_server_queue)
fprint("Creating arm position watcher...")
arm_position_process = pool.apply_async(ur5_control.get_position_thread, (arm,arm_updates))
fprint("Starting websocket server...", sendqueue=to_server_queue) fprint("Starting websocket server...", sendqueue=to_server_queue)
websocket_process = server.start_websocket_server(to_server_queue, from_server_queue) websocket_process = server.start_websocket_server(to_server_queue, from_server_queue)
@ -458,7 +463,7 @@ def get_open_spot(sensordata):
return False return False
def mainloop_server(pool): def mainloop_server(pool, manager):
# NON-blocking loop # NON-blocking loop
global real global real
global ring_animation global ring_animation
@ -484,10 +489,14 @@ def mainloop_server(pool):
global arm_updates global arm_updates
global animation_wait global animation_wait
global arm_position global arm_position
global arm_position_process
if mode != oldmode: if mode != oldmode:
print(" ***** Running mode:", mode, "***** ") print(" ***** Running mode:", mode, "***** ")
oldmode = mode oldmode = mode
if mode == "Startup": # very first loop
pass
if killme.value > 0: if killme.value > 0:
killall() killall()
@ -495,22 +504,27 @@ def mainloop_server(pool):
check_server() check_server()
# do every loop! # do every loop!
checkpoint = None
if not arm_updates.empty():
val = arm_updates.get()
if isinstance(val, tuple):
arm_position = val
else:
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: if ring_animation is not None and ledsys.mode != "idle" and real:
if not animation_wait and not arm_updates.empty(): ledsys.mainloop(None, ring_animation, arm_position=arm_position)
arm_updates.get()
ledsys.mainloop(None, ring_animation)
elif ring_animation is not None and real: elif ring_animation is not None and real:
if animation_wait and not arm_updates.empty(): if animation_wait:
animation_wait = False if checkpoint is not None: # got to checkpoint from UR5
val = None ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
while not arm_updates.empty(): led_set_mode = None
tmp = arm_updates.get() animation_wait = False
if type(tmp) != tuple: else:
val = tmp ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
else:
arm_position = tmp
if val is not None:
ledsys.mainloop(led_set_mode, ring_animation)
led_set_mode = None led_set_mode = None
else: else:
pass pass
@ -525,11 +539,11 @@ def mainloop_server(pool):
if arm_state is None: if arm_state is None:
#pool.apply_async(arm_start_callback, ("",)) #pool.apply_async(arm_start_callback, ("",))
arm_ready = False arm_ready = False
pool.apply_async(ur5_control.holder_to_camera, (arm,counter), callback=arm_start_callback, error_callback=handle_error) pool.apply_async(ur5_control.holder_to_camera, (arm,arm_updates,counter), callback=arm_start_callback, error_callback=handle_error)
fprint("Getting cable index " + str(counter) + " and scanning...") fprint("Getting cable index " + str(counter) + " and scanning...")
arm_state = "GET" arm_state = "GET"
ring_animation = counter ring_animation = counter
animation_wait=True animation_wait = 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
@ -541,6 +555,7 @@ def mainloop_server(pool):
elif camera_ready: elif camera_ready:
ring_animation = counter ring_animation = counter
animation_wait = 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
@ -551,7 +566,7 @@ def mainloop_server(pool):
else: else:
cable_list.append(scan_value) cable_list.append(scan_value)
fprint(scan_value) fprint(scan_value)
pool.apply_async(ur5_control.camera_to_holder, (arm,counter), callback=arm_start_callback, error_callback=handle_error) pool.apply_async(ur5_control.camera_to_holder, (arm,arm_updates,counter), callback=arm_start_callback, error_callback=handle_error)
#ur5_control.return_camera(arm, counter) #ur5_control.return_camera(arm, counter)
#arm_ready = True #arm_ready = True
arm_state = "RETURN" arm_state = "RETURN"
@ -675,7 +690,10 @@ def mainloop_server(pool):
if spot is not False: if spot is not False:
arm_ready = False arm_ready = False
if real: if real:
pool.apply_async(ur5_control.holder_to_tray, (arm, get_cable, spot), callback=arm_start_callback, error_callback=handle_error) animation_wait = False
ring_animation = get_cable
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: else:
arm_ready = True arm_ready = True
fprint("Getting cable at position " + str(get_cable)) fprint("Getting cable at position " + str(get_cable))
@ -687,7 +705,7 @@ def mainloop_server(pool):
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:
pool.apply_async(ur5_control.tray_to_camera, (arm, 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
mode = "ReturnC" mode = "ReturnC"
@ -703,6 +721,9 @@ def mainloop_server(pool):
else: else:
# getting cable and bringing to tray # getting cable and bringing to tray
# led animation # led animation
if ledsys.mode == "idle":
animation_wait = True
led_set_mode = "GrabAB"
pass pass
if mode == "ReturnC": if mode == "ReturnC":
@ -712,6 +733,9 @@ def mainloop_server(pool):
arm_ready = False arm_ready = False
camera_ready = False camera_ready = False
if real: if real:
animation_wait = False
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, (10,), callback=camera_start_callback, error_callback=handle_error)
else: else:
camera_ready = True camera_ready = True
@ -742,6 +766,9 @@ def mainloop_server(pool):
cable_list_state[idx] = True # mark cable as returned cable_list_state[idx] = True # mark cable as returned
arm_ready = False arm_ready = False
if real: if real:
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) pool.apply_async(ur5_control.camera_to_holder, (arm, idx), callback=arm_start_callback, error_callback=handle_error)
else: else:
arm_ready = True arm_ready = True
@ -751,6 +778,9 @@ def mainloop_server(pool):
fprint("WARNING: Holder still marked as occupied!") fprint("WARNING: Holder still marked as occupied!")
arm_ready = False arm_ready = False
if real: if real:
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) pool.apply_async(ur5_control.camera_to_holder, (arm, idx), callback=arm_start_callback, error_callback=handle_error)
else: else:
arm_ready = True arm_ready = True
@ -911,13 +941,13 @@ if __name__ == "__main__":
elif config["core"]["mode"] == "linuxserver": elif config["core"]["mode"] == "linuxserver":
fprint("Starting in server mode.") fprint("Starting in server mode.")
if setup_server(pool): if setup_server(pool, manager):
fprint("Entering main loop...") fprint("Entering main loop...")
start = 0 start = 0
speed = config["core"]["loopspeed"] speed = config["core"]["loopspeed"]
while(keeprunning): while(keeprunning):
start = uptime() start = uptime()
mainloop_server(pool) mainloop_server(pool, manager)
#sleep(0.01) #sleep(0.01)
# limit to certain "framerate" # limit to certain "framerate"
#print(start, start + 1.0/speed, uptime()) #print(start, start + 1.0/speed, uptime())

51
ur5_control.py
View File

@ -21,7 +21,7 @@ class Rob():
#offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset #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 self.config = config
armc = config["arm"] armc = config["arm"]
self.ip = armc["ip"] self.ip = armc["ip"]
@ -33,7 +33,7 @@ class Rob():
self.limb2 = limbs["limb2"] self.limb2 = limbs["limb2"]
self.limb3 = limbs["limb3"] self.limb3 = limbs["limb3"]
self.limb_wrist = limbs["limb_wrist"] self.limb_wrist = limbs["limb_wrist"]
self.pos_updates = pos_queue
#self.init_arm() #self.init_arm()
def ping(host): 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) 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 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) robot = connect(robot)
rob = robot.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) 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)
robot.pos_updates.put(1) pos_updates.put(1)
fprint("Triggering LED interface")
if pick_up: if pick_up:
open_gripper() open_gripper()
@ -531,12 +532,13 @@ def holder_routine(robot, 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)
robot.pos_updates.put(2) pos_updates.put(2)
fprint("Triggering LED interface")
# goto_holder_index(robot, 25, z=0.2) # goto_holder_index(robot, 25, z=0.2)
def pick_up_holder(robot, holder_index, verbose=False): def pick_up_holder(robot, pos_updates, holder_index, verbose=False):
holder_routine(robot, holder_index, True, verbose=verbose) holder_routine(robot, pos_updates, holder_index, True, verbose=verbose)
def drop_off_holder(robot, holder_index, verbose=False): def drop_off_holder(robot, pos_updates, holder_index, verbose=False):
holder_routine(robot, holder_index, False, verbose=verbose) holder_routine(robot, pos_updates, holder_index, False, verbose=verbose)
def tray_routine(robot, slot=0, pick_up=True): def tray_routine(robot, slot=0, pick_up=True):
robot = connect(robot) robot = connect(robot)
@ -648,29 +650,29 @@ def return_routine(robot, slot, holder_index=None, verbose=False):
return True return True
def goto_camera(robot): def goto_camera(robot, pos_updates):
robot = connect(robot) robot = connect(robot)
goto_holder_index(robot, 49, 0.2) 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) pick_up_tray(robot, slot)
goto_camera(robot) goto_camera(robot, pos_updates)
def holder_to_tray(robot, holder_index, slot): def holder_to_tray(robot, pos_updates, holder_index, slot):
pick_up_holder(robot, holder_index) pick_up_holder(robot, pos_updates, holder_index)
drop_off_tray(robot, slot) 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) robot = connect(robot)
fprint("Bringing tube at " + str(holder_index) + " to camera") fprint("Bringing tube at " + str(holder_index) + " to camera")
rob = robot.robot rob = robot.robot
pick_up_holder(robot, holder_index) pick_up_holder(robot, pos_updates, holder_index)
goto_camera(robot) goto_camera(robot, pos_updates)
def camera_to_holder(robot, holder_index, verbose=False): def camera_to_holder(robot, pos_updates, holder_index, verbose=False):
robot = connect(robot) robot = connect(robot)
rob = robot.robot rob = robot.robot
drop_off_holder(robot, holder_index) drop_off_holder(robot, pos_updates, holder_index)
def open_gripper(): def open_gripper():
@ -699,15 +701,18 @@ def close_gripper():
c.close() c.close()
# #
def get_position_thread(robot): def get_position_thread(robot, pos_updates):
robot = connect(robot) robot = connect(robot)
rob = robot.robot rob = robot.robot
vals = rob.getl() while True:
robot.pos_updates.put(tuple(vals)) 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__": if __name__ == "__main__":
#init("192.168.1.145")
with open('config.yml', 'r') as fileread: with open('config.yml', 'r') as fileread:
#global config #global config
config = yaml.safe_load(fileread) config = yaml.safe_load(fileread)