Update LED system
This commit is contained in:
88
run.py
88
run.py
@@ -67,9 +67,10 @@ ring_animation = None
|
||||
led_set_mode = None
|
||||
sensors = [0,0,0,0]
|
||||
websocket_process = None
|
||||
arm_updates = Queue()
|
||||
arm_updates = None
|
||||
animation_wait = False
|
||||
arm_position = (0,0,0,0,0,0)
|
||||
arm_position_process = None
|
||||
|
||||
def arm_start_callback(res):
|
||||
fprint("Arm action complete.")
|
||||
@@ -192,7 +193,7 @@ def check_server():
|
||||
tmp1["application"] = key
|
||||
else:
|
||||
tmp1["application"] = fs["Product Overview"]["Suitable Applications:"]
|
||||
|
||||
|
||||
if "image" in cabledata:
|
||||
tmp1["image"] = cabledata["image"]
|
||||
if "description" in cabledata:
|
||||
@@ -332,7 +333,7 @@ def check_server_online(serverip, clientip):
|
||||
return True
|
||||
|
||||
|
||||
def setup_server(pool):
|
||||
def setup_server(pool, manager):
|
||||
# linux server setup
|
||||
global config
|
||||
global counter
|
||||
@@ -348,14 +349,15 @@ def setup_server(pool):
|
||||
global from_server_queue
|
||||
global websocket_process
|
||||
global arm_updates
|
||||
|
||||
|
||||
arm = Rob(config, arm_updates)
|
||||
global arm_position_process
|
||||
global ledsys
|
||||
arm_updates = manager.Queue()
|
||||
arm = Rob(config)
|
||||
if real:
|
||||
pool.apply_async(ur5_control.powerup_arm, (arm,), callback=arm_start_callback, error_callback=handle_error)
|
||||
else:
|
||||
arm_ready = True
|
||||
global ledsys
|
||||
|
||||
if real:
|
||||
ledsys = LEDSystem()
|
||||
#pool.apply_async(ledsys.init, callback=led_start_callback)
|
||||
@@ -394,6 +396,9 @@ def setup_server(pool):
|
||||
ur5_control.init_arm(arm)
|
||||
fprint("Arm initialized.", sendqueue=to_server_queue)
|
||||
|
||||
fprint("Creating arm position watcher...")
|
||||
arm_position_process = pool.apply_async(ur5_control.get_position_thread, (arm,arm_updates))
|
||||
|
||||
fprint("Starting websocket server...", sendqueue=to_server_queue)
|
||||
websocket_process = server.start_websocket_server(to_server_queue, from_server_queue)
|
||||
|
||||
@@ -458,7 +463,7 @@ def get_open_spot(sensordata):
|
||||
return False
|
||||
|
||||
|
||||
def mainloop_server(pool):
|
||||
def mainloop_server(pool, manager):
|
||||
# NON-blocking loop
|
||||
global real
|
||||
global ring_animation
|
||||
@@ -484,10 +489,14 @@ def mainloop_server(pool):
|
||||
global arm_updates
|
||||
global animation_wait
|
||||
global arm_position
|
||||
global arm_position_process
|
||||
|
||||
if mode != oldmode:
|
||||
print(" ***** Running mode:", mode, "***** ")
|
||||
oldmode = mode
|
||||
if mode == "Startup": # very first loop
|
||||
pass
|
||||
|
||||
if killme.value > 0:
|
||||
killall()
|
||||
|
||||
@@ -495,22 +504,27 @@ def mainloop_server(pool):
|
||||
check_server()
|
||||
|
||||
# do every loop!
|
||||
checkpoint = None
|
||||
if not arm_updates.empty():
|
||||
val = arm_updates.get()
|
||||
|
||||
if isinstance(val, tuple):
|
||||
arm_position = val
|
||||
else:
|
||||
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:
|
||||
if not animation_wait and not arm_updates.empty():
|
||||
arm_updates.get()
|
||||
ledsys.mainloop(None, ring_animation)
|
||||
ledsys.mainloop(None, ring_animation, arm_position=arm_position)
|
||||
|
||||
elif ring_animation is not None and real:
|
||||
if animation_wait and not arm_updates.empty():
|
||||
animation_wait = False
|
||||
val = None
|
||||
while not arm_updates.empty():
|
||||
tmp = arm_updates.get()
|
||||
if type(tmp) != tuple:
|
||||
val = tmp
|
||||
else:
|
||||
arm_position = tmp
|
||||
if val is not None:
|
||||
ledsys.mainloop(led_set_mode, ring_animation)
|
||||
if animation_wait:
|
||||
if checkpoint is not None: # got to checkpoint from UR5
|
||||
ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
|
||||
led_set_mode = None
|
||||
animation_wait = False
|
||||
else:
|
||||
ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
|
||||
led_set_mode = None
|
||||
else:
|
||||
pass
|
||||
@@ -525,11 +539,11 @@ def mainloop_server(pool):
|
||||
if arm_state is None:
|
||||
#pool.apply_async(arm_start_callback, ("",))
|
||||
arm_ready = False
|
||||
pool.apply_async(ur5_control.holder_to_camera, (arm,counter), callback=arm_start_callback, error_callback=handle_error)
|
||||
pool.apply_async(ur5_control.holder_to_camera, (arm,arm_updates,counter), callback=arm_start_callback, error_callback=handle_error)
|
||||
fprint("Getting cable index " + str(counter) + " and scanning...")
|
||||
arm_state = "GET"
|
||||
ring_animation = counter
|
||||
animation_wait=True
|
||||
animation_wait = True
|
||||
led_set_mode = "GrabA"
|
||||
#ur5_control.to_camera(arm, counter)
|
||||
#arm_ready = True
|
||||
@@ -541,6 +555,7 @@ def mainloop_server(pool):
|
||||
|
||||
elif camera_ready:
|
||||
ring_animation = counter
|
||||
animation_wait = True
|
||||
led_set_mode = "GrabC"
|
||||
fprint("Adding cable to list...")
|
||||
global scan_value
|
||||
@@ -551,7 +566,7 @@ def mainloop_server(pool):
|
||||
else:
|
||||
cable_list.append(scan_value)
|
||||
fprint(scan_value)
|
||||
pool.apply_async(ur5_control.camera_to_holder, (arm,counter), callback=arm_start_callback, error_callback=handle_error)
|
||||
pool.apply_async(ur5_control.camera_to_holder, (arm,arm_updates,counter), callback=arm_start_callback, error_callback=handle_error)
|
||||
#ur5_control.return_camera(arm, counter)
|
||||
#arm_ready = True
|
||||
arm_state = "RETURN"
|
||||
@@ -675,7 +690,10 @@ def mainloop_server(pool):
|
||||
if spot is not False:
|
||||
arm_ready = False
|
||||
if real:
|
||||
pool.apply_async(ur5_control.holder_to_tray, (arm, get_cable, spot), callback=arm_start_callback, error_callback=handle_error)
|
||||
animation_wait = False
|
||||
ring_animation = get_cable
|
||||
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:
|
||||
arm_ready = True
|
||||
fprint("Getting cable at position " + str(get_cable))
|
||||
@@ -687,7 +705,7 @@ def mainloop_server(pool):
|
||||
arm_ready = False
|
||||
fprint("Returning cable from tray position " + str(get_cable))
|
||||
if real:
|
||||
pool.apply_async(ur5_control.tray_to_camera, (arm, get_cable), callback=arm_start_callback, error_callback=handle_error)
|
||||
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
|
||||
mode = "ReturnC"
|
||||
@@ -703,6 +721,9 @@ def mainloop_server(pool):
|
||||
else:
|
||||
# getting cable and bringing to tray
|
||||
# led animation
|
||||
if ledsys.mode == "idle":
|
||||
animation_wait = True
|
||||
led_set_mode = "GrabAB"
|
||||
pass
|
||||
|
||||
if mode == "ReturnC":
|
||||
@@ -712,6 +733,9 @@ def mainloop_server(pool):
|
||||
arm_ready = False
|
||||
camera_ready = False
|
||||
if real:
|
||||
animation_wait = False
|
||||
ring_animation = 49
|
||||
led_set_mode = "Camera"
|
||||
pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
|
||||
else:
|
||||
camera_ready = True
|
||||
@@ -742,6 +766,9 @@ def mainloop_server(pool):
|
||||
cable_list_state[idx] = True # mark cable as returned
|
||||
arm_ready = False
|
||||
if real:
|
||||
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)
|
||||
else:
|
||||
arm_ready = True
|
||||
@@ -751,6 +778,9 @@ def mainloop_server(pool):
|
||||
fprint("WARNING: Holder still marked as occupied!")
|
||||
arm_ready = False
|
||||
if real:
|
||||
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)
|
||||
else:
|
||||
arm_ready = True
|
||||
@@ -911,13 +941,13 @@ if __name__ == "__main__":
|
||||
|
||||
elif config["core"]["mode"] == "linuxserver":
|
||||
fprint("Starting in server mode.")
|
||||
if setup_server(pool):
|
||||
if setup_server(pool, manager):
|
||||
fprint("Entering main loop...")
|
||||
start = 0
|
||||
speed = config["core"]["loopspeed"]
|
||||
while(keeprunning):
|
||||
start = uptime()
|
||||
mainloop_server(pool)
|
||||
mainloop_server(pool, manager)
|
||||
#sleep(0.01)
|
||||
# limit to certain "framerate"
|
||||
#print(start, start + 1.0/speed, uptime())
|
||||
|
||||
Reference in New Issue
Block a user