Improve simulation mode
This commit is contained in:
parent
d0083ed33f
commit
72ab357dbf
162
run.py
162
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.")
|
||||
@ -110,30 +112,13 @@ def send_data(type, call, data, client_id="*"):
|
||||
out["data"] = data
|
||||
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:
|
||||
# check for messages
|
||||
check_server()
|
||||
|
||||
# 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")
|
||||
# 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)
|
||||
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
|
||||
|
||||
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
|
||||
|
||||
|
||||
|
||||
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")
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user