diff --git a/run.py b/run.py index 3851675..bd71481 100755 --- a/run.py +++ b/run.py @@ -52,6 +52,7 @@ to_server_queue = Queue() from_server_queue = Queue() mainloop_get = Queue() mode = "Startup" +oldmode = "Startup" counter = 0 jbs = None scan_value = None @@ -63,6 +64,7 @@ just_placed = -1 ring_animation = None led_set_mode = None sensors = [0,0,0,0] +websocket_process = None def arm_start_callback(res): fprint("Arm action complete.") @@ -108,32 +110,15 @@ def send_data(type, call, data, client_id="*"): out["type"] = type out["call"] = call out["data"] = data - to_server_queue.put((client_id, json.dumps(out))) + to_server_queue.put((client_id, json.dumps(out))) -def start_server_socket(cable_list): - global jbs - """app = Flask(__name__) - - @app.route('/report_ip', methods=['POST']) - def report_ip(): - client_ip = request.json.get('ip') - fprint(f"Received IP: {client_ip}") - # You can store or process the IP address as needed - return "IP Received", 200 - - app.run(host='0.0.0.0', port=5000)""" +def check_server(): + #print("HI") + global cable_list global to_server_queue global from_server_queue - fprint("Starting WebSocket server...") - websocket_process = server.start_websocket_server(to_server_queue, from_server_queue) - - # Example - #to_server_queue.put("Hello, WebSocket clients!") - - - while True: - #print("HI") - + global jbs + if True: # Handeling Server Requests Loop, will run forever if not from_server_queue.empty(): @@ -145,17 +130,17 @@ def start_server_socket(cable_list): decoded = json.loads(message) except: fprint("Non-JSON message recieved") - continue + return if "type" not in decoded: fprint("Missing \"type\" field.") - continue + return if "call" not in decoded: fprint("Missing \"call\" field.") - continue + return if "data" not in decoded: fprint("Missing \"data\" field.") - continue + return # if we get here, we have a "valid" data packet data = decoded["data"] call = decoded["call"] @@ -247,13 +232,18 @@ def start_server_socket(cable_list): 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"]: + fprint("Adding cable to dispense queue") mainloop_get.put(("pickup", cableidx)) elif "position" in data: + fprint("Adding cable to dispense queue") mainloop_get.put(("pickup", data["position"])) + else: + fprint("Invalid data.") case _: fprint("Unknown/unimplemented data type: " + decoded["type"]) @@ -311,6 +301,10 @@ def setup_server(pool): global camera global arm global jbs + global to_server_queue + global from_server_queue + global websocket_process + arm = Rob(config) if real: @@ -318,7 +312,8 @@ def setup_server(pool): else: arm_ready = True global ledsys - ledsys = LEDSystem() + if real: + ledsys = LEDSystem() #pool.apply_async(ledsys.init, callback=led_start_callback) #pool.apply_async(sensor_control.init, callback=sensor_start_callback) jbs = JukeboxSearch() @@ -327,14 +322,16 @@ def setup_server(pool): if led_ready is False: fprint("waiting for " + "LED controller initialization" + " to complete...", sendqueue=to_server_queue) - ledsys.init() + if real: + ledsys.init() led_ready = True fprint("LED controllers initialized.", sendqueue=to_server_queue) if sensor_ready is False: fprint("waiting for " + "Sensor Initialization" + " to complete...", sendqueue=to_server_queue) global mbconn - mbconn = ModbusClient(host="localhost", port=502, unit_id=1, auto_open=True, auto_close=True) + mbconn = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=True) + get_sensors() fprint("Sensors initialized.", sendqueue=to_server_queue) if camera_ready is False: @@ -353,7 +350,8 @@ def setup_server(pool): ur5_control.init_arm(arm) fprint("Arm initialized.", sendqueue=to_server_queue) - + fprint("Starting websocket server...", sendqueue=to_server_queue) + websocket_process = server.start_websocket_server(to_server_queue, from_server_queue) return True @@ -364,6 +362,8 @@ def get_sensors(): global mbconn global sensors oldsens = sensors + #print("Reading sensors") + #mbconn.open() """ port 1: 256 port 2: 272 @@ -377,17 +377,19 @@ def get_sensors(): """ out = list() for reg in [352, 288, 304, 368]: - val = mbconn.read_holding_registers(reg) + val = mbconn.read_holding_registers(reg)[0] if val == 1: out.append(1) else: out.append(0) sensors = out - + #fprint("Values: " + str(sensors)) + #mbconn.close() for x in range(len(oldsens)): if oldsens[x] == 0 and out[x] == 1: # cable newly detected on tray + fprint("Precense detected: slot " + str(x)) return x return -1 @@ -424,25 +426,31 @@ def mainloop_server(pool): global mainloop_get global cable_list_state global scan_value - #print(" ***** Running main system loop ***** ") + global oldmode + if mode != oldmode: + print(" ***** Running mode:", mode, "***** ") + oldmode = mode if killme.value > 0: killall() - 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) - led_set_mode = None - else: - pass - #fprint("Not triggering LED loop: no ring animation") + # check for messages + check_server() + + # do every loop! + if ring_animation is not None and ledsys.mode != "idle" and real: + ledsys.mainloop(None, ring_animation) + elif ring_animation is not None and real: + ledsys.mainloop(led_set_mode, ring_animation) + led_set_mode = None + else: + pass + #fprint("Not triggering LED loop: no ring animation") if mode == "Startup": + real = False if not real: counter = 54 + if counter < 54: # scanning cables if arm_state is None: @@ -524,7 +532,7 @@ def mainloop_server(pool): if not real: cable_list = tmp # comment out for real demo - + real = True for idx in range(len(cable_list)): cable_list_state.append(True) @@ -567,8 +575,7 @@ def mainloop_server(pool): fprint("All cables added to database.") mode = "Idle" - serverproc = Process(target=start_server_socket, args=(cable_list,)) - serverproc.start() + else: # TODO: manual input pass @@ -577,13 +584,11 @@ def mainloop_server(pool): if mode == "Idle": # do nothing if arm_ready is False: - if real: - pool.apply_async(ur5_control.move_to_home, (arm,), callback=arm_start_callback, error_callback=handle_error) - else: - arm_ready = True + pass else: global mainloop_get + #print("Checking sensors..") newtube = get_sensors() if newtube >= 0 and newtube != just_placed: # need to return a cable @@ -591,6 +596,7 @@ def mainloop_server(pool): just_placed = -1 if not mainloop_get.empty(): + fprint("Movement requested. Keep clear of the machine!") action, get_cable = mainloop_get.get() if get_cable > -1: global sensors @@ -623,7 +629,6 @@ def mainloop_server(pool): # complete if arm_ready == True: mode = "Idle" - arm_ready = False else: # getting cable and bringing to tray @@ -658,26 +663,37 @@ def mainloop_server(pool): 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 - if real: - pool.apply_async(ur5_control.camera_to_holder, (arm, idx), callback=arm_start_callback, error_callback=handle_error) - else: - arm_ready = True - mode = "Return" - break + fprint("Got cable: " + str(scan_value)) + if scan_value[0:2] == "BL" or scan_value[0:2] == "AW": + scan_value = scan_value[2:] + 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 + if real: + pool.apply_async(ur5_control.camera_to_holder, (arm, idx), callback=arm_start_callback, error_callback=handle_error) + else: + arm_ready = True + mode = "Return" + break + elif cable == scan_value and cable_list_state[idx] == True: + fprint("WARNING: Holder still marked as occupied!") + arm_ready = False + if real: + pool.apply_async(ur5_control.camera_to_holder, (arm, idx), callback=arm_start_callback, error_callback=handle_error) + else: + arm_ready = True + mode = "Return" + break + if mode == "Return": if arm_ready == True: mode = "Idle" - arm_ready = False + #arm_ready = False # movement finished else: @@ -824,24 +840,22 @@ if __name__ == "__main__": mainloop_client(pool) elif config["core"]["mode"] == "linuxserver": - fprint("Starting in server mode.") - if setup_server(pool): - fprint("Entering main loop...") - while(keeprunning): - mainloop_server(pool) - else: - fprint("Mode unspecified - assuming server") fprint("Starting in server mode.") if setup_server(pool): fprint("Entering main loop...") start = 0 - speed = config["loopspeed"] + speed = config["core"]["loopspeed"] while(keeprunning): start = uptime() mainloop_server(pool) #sleep(0.01) # limit to certain "framerate" - while start + 1.0/speed < uptime(): - pass + #print(start, start + 1.0/speed, uptime()) + while start + 1.0/speed > uptime(): + sleep(0.001) + else: + fprint("Mode unspecified - quitting") + +