prerelease
This commit is contained in:
107
run.py
107
run.py
@@ -32,7 +32,7 @@ from uptime import uptime
|
||||
import fileserver
|
||||
|
||||
# set to false to run without real hardware for development
|
||||
real = False
|
||||
real = True
|
||||
skip_scanning = True
|
||||
|
||||
mbconn = None
|
||||
@@ -71,6 +71,8 @@ arm_updates = None
|
||||
animation_wait = False
|
||||
arm_position = (0,0,0,0,0,0)
|
||||
arm_position_process = None
|
||||
start_animation = False
|
||||
failcount = 0
|
||||
|
||||
def arm_start_callback(res):
|
||||
fprint("Arm action complete.")
|
||||
@@ -531,6 +533,8 @@ def mainloop_server(pool, manager):
|
||||
global animation_wait
|
||||
global arm_position
|
||||
global arm_position_process
|
||||
global start_animation
|
||||
global failcount
|
||||
|
||||
if mode != oldmode:
|
||||
print(" ***** Running mode:", mode, "***** ")
|
||||
@@ -555,23 +559,64 @@ def mainloop_server(pool, manager):
|
||||
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:
|
||||
ledsys.mainloop(None, ring_animation, arm_position=arm_position)
|
||||
|
||||
|
||||
elif ring_animation is not None and real:
|
||||
if animation_wait:
|
||||
if checkpoint is not None: # got to checkpoint from UR5
|
||||
fprint("Starting checkpointed animation " + led_set_mode + " for ring " + str(ring_animation))
|
||||
if start_animation:
|
||||
# animation start requested
|
||||
# may not be immediate
|
||||
if ring_animation is not None:
|
||||
if animation_wait:
|
||||
# wait for checkpoint
|
||||
if checkpoint is not None:
|
||||
fprint("Starting checkpointed animation " + str(led_set_mode) + " for ring " + str(ring_animation))
|
||||
ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
|
||||
led_set_mode = None
|
||||
animation_wait = False
|
||||
start_animation = False
|
||||
|
||||
else:
|
||||
# still waiting
|
||||
ledsys.mainloop(None, ring_animation, arm_position=arm_position)
|
||||
|
||||
else:
|
||||
# no waiting, just start
|
||||
fprint("Starting immediate animation " + str(led_set_mode) + " for ring " + str(ring_animation))
|
||||
ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
|
||||
led_set_mode = None
|
||||
animation_wait = False
|
||||
start_animation = False
|
||||
else:
|
||||
fprint("Starting immediate animation " + led_set_mode + " for ring " + str(ring_animation))
|
||||
ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
|
||||
led_set_mode = None
|
||||
# no ring animation specified
|
||||
pass
|
||||
|
||||
else:
|
||||
pass
|
||||
#fprint("Not triggering LED loop: no ring animation")
|
||||
# no new animation
|
||||
if ring_animation is not None:
|
||||
ledsys.mainloop(None, ring_animation, arm_position=arm_position)
|
||||
|
||||
else:
|
||||
pass
|
||||
|
||||
# if start_animation is False and ring_animation is not None and ledsys.mode != "Idle" and real:
|
||||
# ledsys.mainloop(None, ring_animation, arm_position=arm_position)
|
||||
|
||||
# elif start_animation is True and ring_animation is not None and real:
|
||||
# if animation_wait:
|
||||
# if checkpoint is not None: # got to checkpoint from UR5
|
||||
# fprint("Starting checkpointed animation " + str(led_set_mode) + " for ring " + str(ring_animation))
|
||||
# ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
|
||||
# led_set_mode = None
|
||||
# animation_wait = False
|
||||
# start_animation = False
|
||||
# else:
|
||||
# fprint("Starting immediate animation " + str(led_set_mode) + " for ring " + str(ring_animation))
|
||||
# ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
|
||||
# led_set_mode = None
|
||||
# start_animation = False
|
||||
# else:
|
||||
# ledsys.mainloop(None, 49, arm_position=arm_position)
|
||||
# pass
|
||||
# #fprint("Not triggering LED loop: no ring animation")
|
||||
|
||||
if mode == "Startup":
|
||||
if not real or skip_scanning:
|
||||
@@ -587,6 +632,7 @@ def mainloop_server(pool, manager):
|
||||
arm_state = "GET"
|
||||
ring_animation = counter
|
||||
animation_wait = True
|
||||
start_animation = True
|
||||
led_set_mode = "GrabA"
|
||||
#ur5_control.to_camera(arm, counter)
|
||||
#arm_ready = True
|
||||
@@ -599,6 +645,7 @@ def mainloop_server(pool, manager):
|
||||
elif camera_ready:
|
||||
ring_animation = counter
|
||||
animation_wait = True
|
||||
start_animation = True
|
||||
led_set_mode = "GrabC"
|
||||
fprint("Adding cable to list...")
|
||||
global scan_value
|
||||
@@ -624,7 +671,8 @@ def mainloop_server(pool, manager):
|
||||
else:
|
||||
# scanned everything
|
||||
ring_animation = None
|
||||
led_set_mode == "idle"
|
||||
led_set_mode == "Idle"
|
||||
start_animation = True
|
||||
tmp = [
|
||||
# Actual cables in Jukebox
|
||||
"BLTF-1LF-006-RS5",
|
||||
@@ -735,6 +783,7 @@ def mainloop_server(pool, manager):
|
||||
if real:
|
||||
animation_wait = False
|
||||
ring_animation = get_cable
|
||||
start_animation = True
|
||||
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:
|
||||
@@ -748,6 +797,7 @@ def mainloop_server(pool, manager):
|
||||
arm_ready = False
|
||||
fprint("Returning cable from tray position " + str(get_cable))
|
||||
if real:
|
||||
failcount = 0
|
||||
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
|
||||
@@ -764,8 +814,9 @@ def mainloop_server(pool, manager):
|
||||
else:
|
||||
# getting cable and bringing to tray
|
||||
# led animation
|
||||
if ledsys.mode == "idle":
|
||||
if ledsys.mode == "Idle" and led_set_mode != "GrabAA":
|
||||
animation_wait = True
|
||||
start_animation = True
|
||||
led_set_mode = "GrabAB"
|
||||
pass
|
||||
|
||||
@@ -777,9 +828,10 @@ def mainloop_server(pool, manager):
|
||||
camera_ready = False
|
||||
if real:
|
||||
animation_wait = False
|
||||
start_animation = True
|
||||
ring_animation = 49
|
||||
led_set_mode = "Camera"
|
||||
pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
|
||||
pool.apply_async(camera.read_qr, (50,), callback=camera_start_callback, error_callback=handle_error)
|
||||
else:
|
||||
camera_ready = True
|
||||
scan_value = "10GXS13"
|
||||
@@ -793,10 +845,18 @@ def mainloop_server(pool, manager):
|
||||
if camera_ready == True:
|
||||
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
|
||||
if failcount > 15:
|
||||
mode = "Idle"
|
||||
fprint("Giving up scanning cable.")
|
||||
failcount = 0
|
||||
else:
|
||||
fprint("Unable to scan cable. Gonna retry.")
|
||||
camera_ready = False
|
||||
#mode = "Idle"
|
||||
failcount += 1
|
||||
|
||||
pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
|
||||
|
||||
elif scan_value.find("bldn.app/") > -1:
|
||||
scan_value = scan_value[scan_value.find("bldn.app/")+9:]
|
||||
|
||||
@@ -812,7 +872,8 @@ def mainloop_server(pool, manager):
|
||||
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)
|
||||
start_animation = True
|
||||
pool.apply_async(ur5_control.camera_to_holder, (arm, arm_updates, idx), callback=arm_start_callback, error_callback=handle_error)
|
||||
else:
|
||||
arm_ready = True
|
||||
mode = "Return"
|
||||
@@ -824,12 +885,14 @@ def mainloop_server(pool, manager):
|
||||
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)
|
||||
start_animation = True
|
||||
pool.apply_async(ur5_control.camera_to_holder, (arm, arm_updates, idx), callback=arm_start_callback, error_callback=handle_error)
|
||||
else:
|
||||
arm_ready = True
|
||||
mode = "Return"
|
||||
break
|
||||
|
||||
if mode == "Scan":
|
||||
mode = "Idle"
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user