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

69
led_control.py
View File

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

88
run.py
View File

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