Jukebox/jukebox-software
10
0
Fork 0
Code Issues 3 Pull Requests Packages Projects Releases 2 Wiki Activity

Improve simulation mode

Browse Source
This commit is contained in:
Cole Deck 2024-04-29 17:19:52 -05:00
parent d0083ed33f
commit 72ab357dbf
1 changed files with 91 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

168
run.py
View File

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