diff --git a/led_control.py b/led_control.py index 403a61b..d4bc682 100755 --- a/led_control.py +++ b/led_control.py @@ -221,13 +221,14 @@ class LEDSystem(): self.data.append((0,0,0)) self.sendall(self.data) #time.sleep(50000) - fprint("Running start-up test sequence...") - for y in range(1): - for x in range(len(self.leds)): - self.setpixel(0,60,144,x) - self.sendall(self.data) - #time.sleep(2) - self.alloffsmooth() + # fprint("Running start-up test sequence...") + # for y in range(1): + # for x in range(len(self.leds)): + # self.setpixel(0,60,144,x) + # self.sendall(self.data) + # #time.sleep(2) + # self.alloffsmooth() + self.startup_animation(show=False) def sendall(self, datain): # send all LED data to all controllers diff --git a/run.py b/run.py index be09c92..b398965 100755 --- a/run.py +++ b/run.py @@ -27,8 +27,11 @@ import process_video import search from search import JukeboxSearch #multiprocessing.set_start_method('spawn', True) +from pyModbusTCP.client import ModbusClient +from uptime import uptime +mbconn = None config = None keeprunning = True arm_ready = False @@ -45,6 +48,7 @@ ledsys = None arm = None to_server_queue = Queue() from_server_queue = Queue() +mainloop_get = Queue() mode = "Startup" counter = 0 jbs = None @@ -52,10 +56,16 @@ scan_value = None arm_state = None cable_list = list() parse_res = None +cable_list_state = list() +just_placed = -1 +ring_animation = None +led_set_mode = None def arm_start_callback(res): + fprint("Arm action complete.") global arm_ready arm_ready = True + def led_start_callback(res): global led_ready @@ -229,6 +239,19 @@ def start_server_socket(cable_list): pass elif call == "request": pass + + case "cable_get": + fprint("cable_get message") + if call == "send": + global mainloop_get + if "part_number" in data: + for cableidx in range(len(cable_list)): + cable = cable_list[cableidx] + if cable == data["part_number"]: + mainloop_get.put(("pickup", cableidx)) + elif "position" in data: + mainloop_get.put(("pickup", data["position"])) + case _: fprint("Unknown/unimplemented data type: " + decoded["type"]) except Exception as e: @@ -285,11 +308,12 @@ def setup_server(pool): global camera global arm global jbs + arm = Rob(config) - pool.apply_async(arm.init_arm, callback=arm_start_callback) + pool.apply_async(ur5_control.powerup_arm, (arm,), callback=arm_start_callback, error_callback=handle_error) global ledsys ledsys = LEDSystem() - pool.apply_async(ledsys.init, callback=led_start_callback) + #pool.apply_async(ledsys.init, callback=led_start_callback) #pool.apply_async(sensor_control.init, callback=sensor_start_callback) jbs = JukeboxSearch() @@ -297,38 +321,85 @@ def setup_server(pool): if led_ready is False: fprint("waiting for " + "LED controller initialization" + " to complete...", sendqueue=to_server_queue) - while led_ready is False: - sleep(0.1) + ledsys.init() + led_ready = True fprint("LED controllers initialized.", sendqueue=to_server_queue) - #to_server_queue.put("[log] LED controllers initialized.") - sensor_ready = True + if sensor_ready is False: fprint("waiting for " + "Sensor Initialization" + " to complete...", sendqueue=to_server_queue) - while sensor_ready is False: - sleep(0.1) + global mbconn + mbconn = ModbusClient(host="localhost", port=502, unit_id=1, auto_open=True, auto_close=True) fprint("Sensors initialized.", sendqueue=to_server_queue) if camera_ready is False: fprint("waiting for " + "Camera initilization" + " to complete...", sendqueue=to_server_queue) - camera = process_video.qr_reader(config["cameras"]["banner"]["ip"], config["cameras"]["banner"]["port"]) + camera = process_video.qr_reader(config["cameras"]["banner"]["ip"], int(config["cameras"]["banner"]["port"])) fprint("Camera initialized.", sendqueue=to_server_queue) - arm_ready = True + #arm_ready = True if arm_ready is False: - fprint("waiting for " + "UR5 initilization" + " to complete...", sendqueue=to_server_queue) + fprint("waiting for " + "UR5 powerup" + " to complete...", sendqueue=to_server_queue) while arm_ready is False: sleep(0.1) + + ur5_control.init_arm(arm) fprint("Arm initialized.", sendqueue=to_server_queue) return True +def handle_error(error): + print(error, flush=True) +def get_sensors(): + global mbconn + global sensors + oldsens = sensors + """ + port 1: 256 + port 2: 272 + port 3: 288 + port 4: 304 + port 5: 320 + port 6: 336 + port 7: 352 + port 8: 368 + + """ + out = list() + for reg in [352, 288, 304, 368]: + val = mbconn.read_holding_registers(reg) + if val == 1: + out.append(1) + else: + out.append(0) + + sensors = out + + for x in range(len(oldsens)): + if oldsens[x] == 0 and out[x] == 1: + # cable newly detected on tray + return x + + return -1 +def get_open_spot(sensordata): + for x in range(len(sensordata)): + sens = sensordata[x] + if not sens: + return x + + # if we get here, every spot is full + + return False + def mainloop_server(pool): # NON-blocking loop + global ring_animation + global led_set_mode + global just_placed global config global counter global killme @@ -342,26 +413,30 @@ def mainloop_server(pool): global camera_ready global cable_search_ready global cable_list + global mainloop_get + global cable_list_state if killme.value > 0: killall() if mode == "Startup": - counter = 54 + #counter = 54 # remove for demo if counter < 54: # scanning cables - + ring_animation = counter + led_set_mode = "GrabA" if arm_state is None: - #pool.apply_async(arm.get cable to camera, callback=arm_start_callback) - #ur5_control.goto_holder_index(arm) - #ur5 get item - # ur5 bring to camera + #pool.apply_async(arm_start_callback, ("",)) + arm_ready = False + pool.apply_async(ur5_control.to_camera, (arm,counter), callback=arm_start_callback, error_callback=handle_error) fprint("Getting cable index " + str(counter) + " and scanning...") arm_state = "GET" + #ur5_control.to_camera(arm, counter) + #arm_ready = True elif arm_ready and arm_state == "GET": fprint("Looking for QR code...") - pool.apply_async(camera.read_qr, (30,), callback=camera_start_callback) + pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error) arm_ready = False elif camera_ready: @@ -374,7 +449,9 @@ def mainloop_server(pool): else: cable_list.append(scan_value) fprint(scan_value) - #pool.apply_async(arm.return cable, callback=arm_start_callback) + pool.apply_async(ur5_control.return_camera, (arm,counter), callback=arm_start_callback, error_callback=handle_error) + #ur5_control.return_camera(arm, counter) + #arm_ready = True arm_state = "RETURN" camera_ready = False @@ -386,6 +463,8 @@ def mainloop_server(pool): pass else: # scanned everything + ring_animation = None + led_set_mode == "idle" tmp = [ # Actual cables in Jukebox "BLTF-1LF-006-RS5", @@ -418,8 +497,13 @@ def mainloop_server(pool): ] while len(tmp) < 54: tmp.append(False) # must have 54 entries - cable_list = tmp # remove for real demo - pool.apply_async(get_specs.get_multi, (tmp, 0.3), callback=cable_search_callback) + + #cable_list = tmp # comment out for real demo + + for idx in range(len(cable_list)): + cable_list_state.append(True) + + pool.apply_async(get_specs.get_multi, (cable_list, 0.3), callback=cable_search_callback, error_callback=handle_error) mode = "Parsing" fprint("All cables scanned. Finding & parsing datasheets...") if mode == "Parsing": @@ -458,7 +542,7 @@ def mainloop_server(pool): fprint("All cables added to database.") mode = "Idle" - serverproc = Process(target=start_server_socket, args=(cable_list,)) + serverproc = Process(target=start_server_socket, args=(cable_list,), error_callback=handle_error) serverproc.start() else: # TODO: manual input @@ -468,14 +552,105 @@ def mainloop_server(pool): if mode == "Idle": # do nothing if arm_ready is False: - pool.apply_async(ur5_control.move_to_home, (arm,), callback=arm_start_callback) - arm_ready = True + pool.apply_async(ur5_control.move_to_home, (arm,), callback=arm_start_callback, error_callback=handle_error) + #arm_ready = True else: - # LED idle anim + global mainloop_get + newtube = get_sensors() + if newtube >= 0 and newtube != just_placed: + # need to return a cable + mainloop_get.put(("return", newtube)) + just_placed = -1 + + if not mainloop_get.empty(): + action, get_cable = mainloop_get.get() + if get_cable > -1: + global sensors + if action == "pickup": + spot = get_open_spot(sensors) + if spot is not False: + arm_ready = False + pool.apply_async(ur5_control.pick_up_routine, (arm, get_cable, True, spot), callback=arm_start_callback, error_callback=handle_error) + mode = "Pickup" + cable_list_state[get_cable] = False # mark as removed + get_sensors(sensors,flag=get_open_spot(sensors)) + + if action == "return": + arm_ready = False + pool.apply_async(ur5_control.return_routine, (arm, get_cable), callback=arm_start_callback, error_callback=handle_error) + mode = "ReturnC" + else: + # LED idle anim + pass + + if mode == "Pickup": + # complete + if arm_ready == True: + mode = "Idle" + arm_ready = False + + else: + # getting cable and bringing to tray + # led animation + pass + + if mode == "ReturnC": + # complete + if arm_ready == True: + mode = "Scan" + arm_ready = False + camera_ready = False + pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error) + + else: + # getting cable from and bringing to camera + # led animation pass + if mode == "Scan": + if camera_ready == True: + global scan_value + 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 + elif scan_value.find("bldn.app/") > -1: + scan_value = scan_value[scan_value.find("bldn.app/")+9:] + fprint("Got cable: " + scan_value) + + for idx in range(len(cable_list)): + cable = cable_list[idx] + if cable == scan_value and cable_list_state[idx] == False: + cable_list_state[idx] = True # mark cable as returned + arm_ready = False + pool.apply_async(ur5_control.return_camera, (arm, idx), callback=arm_start_callback, error_callback=handle_error) + mode = "Return" + break + + + + if mode == "Return": + if arm_ready == True: + mode = "Idle" + arm_ready = False + # movement finished + + else: + # cable going from camera to holder + # led animation + pass + + if True: + # do every loop! + if ring_animation is not None and ledsys.mode != "idle": + ledsys.mainloop(None, ring_animation) + elif ring_animation is not None: + ledsys.mainloop(led_set_mode, ring_animation) + def run_loading_app(): @@ -584,14 +759,15 @@ class LogExceptions(object): return result class LoggingPool(Pool): - def apply_async(self, func, args=(), kwds={}, callback=None): - return Pool.apply_async(self, LogExceptions(func), args, kwds, callback) + def apply_async(self, func, args=(), kwds={}, callback=None, error_callback=None): + return Pool.apply_async(self, LogExceptions(func), args, kwds, callback, error_callback) if __name__ == "__main__": #sys.stdout = Logger(filename="output.log") #sys.stderr = Logger(filename="output.log") #log_to_stderr(logging.DEBUG) + fprint("Starting Jukebox control system...") with open('config.yml', 'r') as fileread: #global config @@ -622,7 +798,13 @@ if __name__ == "__main__": fprint("Starting in server mode.") if setup_server(pool): fprint("Entering main loop...") + start = 0 + speed = config["loopspeed"] while(keeprunning): + start = uptime() mainloop_server(pool) + # limit to certain "framerate" + while start + 1.0/speed < uptime(): + pass