Simulation mode
This commit is contained in:
56
run.py
56
run.py
@@ -30,6 +30,8 @@ from search import JukeboxSearch
|
||||
from pyModbusTCP.client import ModbusClient
|
||||
from uptime import uptime
|
||||
|
||||
# set to false to run without real hardware for development
|
||||
real = False
|
||||
|
||||
mbconn = None
|
||||
config = None
|
||||
@@ -60,6 +62,7 @@ cable_list_state = list()
|
||||
just_placed = -1
|
||||
ring_animation = None
|
||||
led_set_mode = None
|
||||
sensors = [0,0,0,0]
|
||||
|
||||
def arm_start_callback(res):
|
||||
fprint("Arm action complete.")
|
||||
@@ -310,7 +313,10 @@ def setup_server(pool):
|
||||
global jbs
|
||||
|
||||
arm = Rob(config)
|
||||
pool.apply_async(ur5_control.powerup_arm, (arm,), callback=arm_start_callback, error_callback=handle_error)
|
||||
if real:
|
||||
pool.apply_async(ur5_control.powerup_arm, (arm,), callback=arm_start_callback, error_callback=handle_error)
|
||||
else:
|
||||
arm_ready = True
|
||||
global ledsys
|
||||
ledsys = LEDSystem()
|
||||
#pool.apply_async(ledsys.init, callback=led_start_callback)
|
||||
@@ -343,7 +349,8 @@ def setup_server(pool):
|
||||
while arm_ready is False:
|
||||
sleep(0.1)
|
||||
|
||||
ur5_control.init_arm(arm)
|
||||
if real:
|
||||
ur5_control.init_arm(arm)
|
||||
fprint("Arm initialized.", sendqueue=to_server_queue)
|
||||
|
||||
|
||||
@@ -397,6 +404,7 @@ def get_open_spot(sensordata):
|
||||
|
||||
def mainloop_server(pool):
|
||||
# NON-blocking loop
|
||||
global real
|
||||
global ring_animation
|
||||
global led_set_mode
|
||||
global just_placed
|
||||
@@ -416,7 +424,7 @@ def mainloop_server(pool):
|
||||
global mainloop_get
|
||||
global cable_list_state
|
||||
global scan_value
|
||||
print(" ***** Running main system loop ***** ")
|
||||
#print(" ***** Running main system loop ***** ")
|
||||
if killme.value > 0:
|
||||
killall()
|
||||
|
||||
@@ -429,10 +437,12 @@ def mainloop_server(pool):
|
||||
ledsys.mainloop(led_set_mode, ring_animation)
|
||||
led_set_mode = None
|
||||
else:
|
||||
fprint("Not triggering LED loop: no ring animation")
|
||||
pass
|
||||
#fprint("Not triggering LED loop: no ring animation")
|
||||
|
||||
if mode == "Startup":
|
||||
#counter = 54 # remove for demo
|
||||
if not real:
|
||||
counter = 54
|
||||
if counter < 54:
|
||||
# scanning cables
|
||||
if arm_state is None:
|
||||
@@ -512,7 +522,8 @@ def mainloop_server(pool):
|
||||
while len(tmp) < 54:
|
||||
tmp.append(False) # must have 54 entries
|
||||
|
||||
#cable_list = tmp # comment out for real demo
|
||||
if not real:
|
||||
cable_list = tmp # comment out for real demo
|
||||
|
||||
for idx in range(len(cable_list)):
|
||||
cable_list_state.append(True)
|
||||
@@ -556,7 +567,7 @@ def mainloop_server(pool):
|
||||
|
||||
fprint("All cables added to database.")
|
||||
mode = "Idle"
|
||||
serverproc = Process(target=start_server_socket, args=(cable_list,), error_callback=handle_error)
|
||||
serverproc = Process(target=start_server_socket, args=(cable_list,))
|
||||
serverproc.start()
|
||||
else:
|
||||
# TODO: manual input
|
||||
@@ -566,8 +577,10 @@ def mainloop_server(pool):
|
||||
if mode == "Idle":
|
||||
# do nothing
|
||||
if arm_ready is False:
|
||||
pool.apply_async(ur5_control.move_to_home, (arm,), callback=arm_start_callback, error_callback=handle_error)
|
||||
#arm_ready = True
|
||||
if real:
|
||||
pool.apply_async(ur5_control.move_to_home, (arm,), callback=arm_start_callback, error_callback=handle_error)
|
||||
else:
|
||||
arm_ready = True
|
||||
|
||||
else:
|
||||
global mainloop_get
|
||||
@@ -585,14 +598,22 @@ def mainloop_server(pool):
|
||||
spot = get_open_spot(sensors)
|
||||
if spot is not False:
|
||||
arm_ready = False
|
||||
pool.apply_async(ur5_control.holder_to_tray, (arm, get_cable, spot), callback=arm_start_callback, error_callback=handle_error)
|
||||
if real:
|
||||
pool.apply_async(ur5_control.holder_to_tray, (arm, get_cable, spot), callback=arm_start_callback, error_callback=handle_error)
|
||||
else:
|
||||
arm_ready = True
|
||||
fprint("Getting cable at position " + str(get_cable))
|
||||
mode = "Pickup"
|
||||
cable_list_state[get_cable] = False # mark as removed
|
||||
get_sensors(sensors,flag=get_open_spot(sensors))
|
||||
get_sensors()
|
||||
|
||||
if action == "return":
|
||||
arm_ready = False
|
||||
pool.apply_async(ur5_control.tray_to_camera, (arm, get_cable), callback=arm_start_callback, error_callback=handle_error)
|
||||
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)
|
||||
else:
|
||||
arm_ready = True
|
||||
mode = "ReturnC"
|
||||
else:
|
||||
# LED idle anim
|
||||
@@ -615,7 +636,11 @@ def mainloop_server(pool):
|
||||
mode = "Scan"
|
||||
arm_ready = False
|
||||
camera_ready = False
|
||||
pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
|
||||
if real:
|
||||
pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
|
||||
else:
|
||||
camera_ready = True
|
||||
scan_value = "10GXS13"
|
||||
|
||||
else:
|
||||
# getting cable from and bringing to camera
|
||||
@@ -640,7 +665,10 @@ def mainloop_server(pool):
|
||||
if cable == scan_value and cable_list_state[idx] == False:
|
||||
cable_list_state[idx] = True # mark cable as returned
|
||||
arm_ready = False
|
||||
pool.apply_async(ur5_control.camera_to_holder, (arm, idx), callback=arm_start_callback, error_callback=handle_error)
|
||||
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
|
||||
|
||||
|
||||
Reference in New Issue
Block a user