Update LED system
This commit is contained in:
parent
dd7bc12fe5
commit
f416e25c1c
@ -22,12 +22,13 @@ cables:
|
||||
port: 7900
|
||||
directory: ./cables/ # must include trailing slash
|
||||
|
||||
#cable_map:
|
||||
cameras:
|
||||
banner:
|
||||
ip: 192.168.1.125
|
||||
port: 32200
|
||||
|
||||
animation_time: 100
|
||||
|
||||
led:
|
||||
fps: 100
|
||||
timeout: 1
|
||||
@ -438,7 +439,7 @@ led:
|
||||
# pos: [375, 300]
|
||||
|
||||
global_position_offset: [0,0] # default coordinate spce below as center of arm at 0,0 - adjust if necessary
|
||||
animation_time: 200
|
||||
|
||||
position_map:
|
||||
- index: 0
|
||||
pos: [-152.4, 263.965]
|
||||
|
||||
@ -315,12 +315,13 @@ class LEDSystem():
|
||||
for x in range(start,end):
|
||||
self.data[x] = val
|
||||
|
||||
def setallrings(self, r,g,b, exclude):
|
||||
def setallringsexcept(self, r,g,b, exclude):
|
||||
startidx1 = self.rings[0][2]
|
||||
endidx2 = self.rings[-1][3]
|
||||
endidx1 = self.rings[exclude][2]
|
||||
startidx2 = self.rings[exclude][3]+1
|
||||
self.setrange(startidx, endidx, r,g,b)
|
||||
self.setrange(startidx1, endidx1, r,g,b)
|
||||
self.setrange(startidx2, endidx2, r,g,b)
|
||||
|
||||
|
||||
def setring(self, r,g,b,idx):
|
||||
@ -336,7 +337,7 @@ class LEDSystem():
|
||||
if self.mode == "Startup":
|
||||
# loading animation. cable check
|
||||
if self.firstrun:
|
||||
self.changecount = self.animation_time * 3
|
||||
self.changecount = self.animation_time * 2
|
||||
self.firstrun = False
|
||||
for x in range(len(self.ringstatus)):
|
||||
self.ringstatus[x] = [True, self.animation_time]
|
||||
@ -357,7 +358,8 @@ class LEDSystem():
|
||||
if self.ringstatus[x][0]:
|
||||
self.setring(0, 50, 100, x)
|
||||
else:
|
||||
self.ringstatus[x][1] = self.fadeall(self.rings[x][2],self.rings[x][3], self.ringstatus[x][1], 100,0,0) # not ready
|
||||
#self.ringstatus[x][1] = self.fadeall(self.rings[x][2],self.rings[x][3], self.ringstatus[x][1], 100,0,0) # not ready
|
||||
self.ringstatus[x][1] = self.fadeall(self.rings[x][2],self.rings[x][3], self.ringstatus[x][1], 0,50,100)
|
||||
|
||||
elif self.mode == "StartupCheck":
|
||||
if self.firstrun:
|
||||
@ -389,7 +391,7 @@ class LEDSystem():
|
||||
self.changecount = self.fadeorder(self.rings[ring][2],self.rings[ring][2]+24, self.changecount, 0,100,0)
|
||||
else:
|
||||
self.setring(0,100,0,ring)
|
||||
self.setmode("idle")
|
||||
self.setmode("Moving")
|
||||
elif self.mode == "GrabC":
|
||||
if self.firstrun:
|
||||
self.firstrun = False
|
||||
@ -398,14 +400,35 @@ class LEDSystem():
|
||||
self.changecount = self.fadeall(self.rings[ring][2],self.rings[ring][3], self.changecount, 0,50,100)
|
||||
else:
|
||||
self.setring(0,50,100,ring)
|
||||
self.setmode("Moving")
|
||||
|
||||
elif self.mode == "GrabAA":
|
||||
if self.firstrun:
|
||||
self.firstrun = False
|
||||
self.changecount = self.animation_time # 100hz
|
||||
if self.changecount > 0:
|
||||
self.changecount = self.fadeall(self.rings[ring][2],self.rings[ring][3], self.changecount, 100,0,0)
|
||||
else:
|
||||
self.setring(100,0,0,ring)
|
||||
self.setmode("idle")
|
||||
elif self.mode == "GrabAB":
|
||||
if self.firstrun:
|
||||
self.firstrun = False
|
||||
self.changecount = self.animation_time # 100hz
|
||||
if self.changecount > 0:
|
||||
#self.changecount = self.fadeorder(self.rings[ring][2],self.rings[ring][3], self.changecount, 0,100,0)
|
||||
self.changecount = self.fadeorder(self.rings[ring][2],self.rings[ring][2]+24, self.changecount, 0,100,0)
|
||||
else:
|
||||
self.setring(0,100,0,ring)
|
||||
self.setmode("Moving")
|
||||
|
||||
elif self.mode == "Moving":
|
||||
if self.firstrun:
|
||||
self.firstrun = False
|
||||
posxy = arm_position[0:1]
|
||||
posxy[0] = int(posxy[0] * 1000)
|
||||
posxy[0] = int(posxy[1] * 1000)
|
||||
if arm_position is not None:
|
||||
posxy = list()
|
||||
posxy.append(int(arm_position[0] * 1000)) # x convert m to mm
|
||||
posxy.append(int(arm_position[1] * 1000)) # y
|
||||
radius = int(arm_position[2] * 1000)
|
||||
base = (0,50,100)
|
||||
target = (100,100,100)
|
||||
@ -414,9 +437,11 @@ class LEDSystem():
|
||||
#deltab = target[0] - base[0]
|
||||
# reset!
|
||||
self.setallringsexcept(0,50,100, ring)
|
||||
exclude = self.rings[ring][2],self.rings[ring][3]
|
||||
# fade outwards
|
||||
for idx,led in enumerate(self.leds):
|
||||
dist = int(math.srqt(math.pow(int(posxy[0] - led[0]), 2) + math.pow(int(posxy[1] - led[1]), 2)))
|
||||
if idx < exclude[0] or idx > exclude[1]:
|
||||
dist = int(math.sqrt(math.pow(int(posxy[0] - led[0]), 2) + math.pow(int(posxy[1] - led[1]), 2)))
|
||||
if dist < radius:
|
||||
ratio = dist/radius
|
||||
self.data[idx] = (int(base[0] + ratio * deltar), int(base[1] + ratio * deltag), 100) #base[2] + ratio * deltab)
|
||||
|
||||
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.")
|
||||
@ -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!
|
||||
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)
|
||||
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
|
||||
checkpoint = None
|
||||
if not arm_updates.empty():
|
||||
val = arm_updates.get()
|
||||
|
||||
if isinstance(val, tuple):
|
||||
arm_position = val
|
||||
else:
|
||||
arm_position = tmp
|
||||
if val is not None:
|
||||
ledsys.mainloop(led_set_mode, ring_animation)
|
||||
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
|
||||
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())
|
||||
|
||||
@ -21,7 +21,7 @@ class Rob():
|
||||
#offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset
|
||||
#
|
||||
|
||||
def __init__(self, config, pos_queue=Queue()):
|
||||
def __init__(self, config):
|
||||
self.config = config
|
||||
armc = config["arm"]
|
||||
self.ip = armc["ip"]
|
||||
@ -33,7 +33,7 @@ class Rob():
|
||||
self.limb2 = limbs["limb2"]
|
||||
self.limb3 = limbs["limb3"]
|
||||
self.limb_wrist = limbs["limb_wrist"]
|
||||
self.pos_updates = pos_queue
|
||||
|
||||
#self.init_arm()
|
||||
|
||||
def ping(host):
|
||||
@ -495,7 +495,7 @@ def safe_move(robot, x, y, z, use_closest_path=True):
|
||||
rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot), use_closest_path=use_closest_path), vel=2, acc=2)
|
||||
return True
|
||||
|
||||
def holder_routine(robot, holder_index, pick_up, verbose=False):
|
||||
def holder_routine(robot, pos_updates, holder_index, pick_up, verbose=False):
|
||||
robot = connect(robot)
|
||||
rob = robot.robot
|
||||
|
||||
@ -511,7 +511,8 @@ def holder_routine(robot, holder_index, pick_up, verbose=False):
|
||||
goto_holder_index(robot, holder_index, 0.05, use_closest_path=False)
|
||||
else:
|
||||
goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
|
||||
robot.pos_updates.put(1)
|
||||
pos_updates.put(1)
|
||||
fprint("Triggering LED interface")
|
||||
if pick_up:
|
||||
open_gripper()
|
||||
|
||||
@ -531,12 +532,13 @@ def holder_routine(robot, holder_index, pick_up, verbose=False):
|
||||
new_pos[2] = 0.2
|
||||
rob.movel(new_pos, vel=2, acc=1)
|
||||
was_flipped = is_flipped(robot)
|
||||
robot.pos_updates.put(2)
|
||||
pos_updates.put(2)
|
||||
fprint("Triggering LED interface")
|
||||
# goto_holder_index(robot, 25, z=0.2)
|
||||
def pick_up_holder(robot, holder_index, verbose=False):
|
||||
holder_routine(robot, holder_index, True, verbose=verbose)
|
||||
def drop_off_holder(robot, holder_index, verbose=False):
|
||||
holder_routine(robot, holder_index, False, verbose=verbose)
|
||||
def pick_up_holder(robot, pos_updates, holder_index, verbose=False):
|
||||
holder_routine(robot, pos_updates, holder_index, True, verbose=verbose)
|
||||
def drop_off_holder(robot, pos_updates, holder_index, verbose=False):
|
||||
holder_routine(robot, pos_updates, holder_index, False, verbose=verbose)
|
||||
|
||||
def tray_routine(robot, slot=0, pick_up=True):
|
||||
robot = connect(robot)
|
||||
@ -648,29 +650,29 @@ def return_routine(robot, slot, holder_index=None, verbose=False):
|
||||
return True
|
||||
|
||||
|
||||
def goto_camera(robot):
|
||||
def goto_camera(robot, pos_updates):
|
||||
robot = connect(robot)
|
||||
goto_holder_index(robot, 49, 0.2)
|
||||
|
||||
def tray_to_camera(robot, slot):
|
||||
def tray_to_camera(robot, pos_updates, slot):
|
||||
pick_up_tray(robot, slot)
|
||||
goto_camera(robot)
|
||||
def holder_to_tray(robot, holder_index, slot):
|
||||
pick_up_holder(robot, holder_index)
|
||||
goto_camera(robot, pos_updates)
|
||||
def holder_to_tray(robot, pos_updates, holder_index, slot):
|
||||
pick_up_holder(robot, pos_updates, holder_index)
|
||||
drop_off_tray(robot, slot)
|
||||
|
||||
def holder_to_camera(robot, holder_index, verbose=False):
|
||||
def holder_to_camera(robot, pos_updates, holder_index, verbose=False):
|
||||
robot = connect(robot)
|
||||
fprint("Bringing tube at " + str(holder_index) + " to camera")
|
||||
rob = robot.robot
|
||||
|
||||
pick_up_holder(robot, holder_index)
|
||||
goto_camera(robot)
|
||||
def camera_to_holder(robot, holder_index, verbose=False):
|
||||
pick_up_holder(robot, pos_updates, holder_index)
|
||||
goto_camera(robot, pos_updates)
|
||||
def camera_to_holder(robot, pos_updates, holder_index, verbose=False):
|
||||
robot = connect(robot)
|
||||
rob = robot.robot
|
||||
|
||||
drop_off_holder(robot, holder_index)
|
||||
drop_off_holder(robot, pos_updates, holder_index)
|
||||
|
||||
|
||||
def open_gripper():
|
||||
@ -699,15 +701,18 @@ def close_gripper():
|
||||
c.close()
|
||||
#
|
||||
|
||||
def get_position_thread(robot):
|
||||
def get_position_thread(robot, pos_updates):
|
||||
robot = connect(robot)
|
||||
rob = robot.robot
|
||||
while True:
|
||||
if pos_updates.qsize() < 2:
|
||||
vals = rob.getl()
|
||||
robot.pos_updates.put(tuple(vals))
|
||||
pos_updates.put(tuple(vals))
|
||||
#print("Adding position to queue")
|
||||
time.sleep(0.01)
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
#init("192.168.1.145")
|
||||
with open('config.yml', 'r') as fileread:
|
||||
#global config
|
||||
config = yaml.safe_load(fileread)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user