From 5e0f1e93d221c782715e20c9847f76b2e21c42d6 Mon Sep 17 00:00:00 2001 From: Cole Deck Date: Mon, 13 May 2024 02:21:07 -0500 Subject: [PATCH] prerelease --- config.yml | 12 ++++++ jukebox-web | 2 +- led_control.py | 69 +++++++++++++++++++++---------- run.py | 107 +++++++++++++++++++++++++++++++++++++++---------- ur5_control.py | 38 ++++++++++-------- 5 files changed, 168 insertions(+), 60 deletions(-) diff --git a/config.yml b/config.yml index 73dea48..6a299fa 100644 --- a/config.yml +++ b/config.yml @@ -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 diff --git a/jukebox-web b/jukebox-web index 8824070..8fe3309 160000 --- a/jukebox-web +++ b/jukebox-web @@ -1 +1 @@ -Subproject commit 8824070547e7d183ea3ede4d7040f23aa36c2c2a +Subproject commit 8fe33095ddd16d1b975453b0ee0e7a52767605f8 diff --git a/led_control.py b/led_control.py index 9aba2e7..7eeae7e 100755 --- a/led_control.py +++ b/led_control.py @@ -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 diff --git a/run.py b/run.py index 906d37f..935eb0e 100755 --- a/run.py +++ b/run.py @@ -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" diff --git a/ur5_control.py b/ur5_control.py index b9523ed..dddccaa 100755 --- a/ur5_control.py +++ b/ur5_control.py @@ -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,18 +520,21 @@ 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() else: open_gripper() - + # Move up 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)