From f416e25c1c86f8568f31427479fbee0367abac63 Mon Sep 17 00:00:00 2001 From: Cole Deck Date: Wed, 8 May 2024 20:28:48 -0500 Subject: [PATCH] Update LED system --- config.yml | 5 +-- led_control.py | 69 ++++++++++++++++++++++++++------------- run.py | 88 +++++++++++++++++++++++++++++++++----------------- ur5_control.py | 51 ++++++++++++++++------------- 4 files changed, 137 insertions(+), 76 deletions(-) diff --git a/config.yml b/config.yml index b7149ae..dc79bd7 100644 --- a/config.yml +++ b/config.yml @@ -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] diff --git a/led_control.py b/led_control.py index b5b701b..9040f08 100755 --- a/led_control.py +++ b/led_control.py @@ -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) diff --git a/run.py b/run.py index f9a4898..5a1a162 100755 --- a/run.py +++ b/run.py @@ -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()) diff --git a/ur5_control.py b/ur5_control.py index 4014887..2985432 100755 --- a/ur5_control.py +++ b/ur5_control.py @@ -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)