Jukebox/jukebox-software
10
0
Fork 0
Code Issues 3 Pull Requests Packages Projects Releases 2 Wiki Activity

Add real-time UR5 position updates (hopefully)

Browse Source
This commit is contained in:
Cole Deck 2024-05-09 18:18:55 -05:00
parent f416e25c1c
commit be51423c89
7 changed files with 109 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
config.yml
View File

@ -92,37 +92,37 @@ led:
start: 0
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [0, 304.8]
- type: circle
start: 24
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-65.991, 266.7]
- type: circle
start: 48
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-131.982, 228.6]
- type: circle
start: 72
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-197.973, 190.5]
- type: circle
start: 96
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-263.965, 152.4]
- type: circle
start: 120
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-263.965, 76.2]
# controller 2
@ -130,37 +130,37 @@ led:
start: 144
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [0, 228.6]
- type: circle
start: 168
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-65.991, 190.5]
- type: circle
start: 192
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-131.982, 152.4]
- type: circle
start: 216
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-197.973, 114.3]
- type: circle
start: 240
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-197.973, 38.1]
- type: circle
start: 264
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-263.965, 0]
# controller 3
@ -168,37 +168,37 @@ led:
start: 288
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [0, 152.4]
- type: circle
start: 312
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-65.991, 114.3]
- type: circle
start: 336
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-131.982, 76.2]
- type: circle
start: 360
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-131.982, 0]
- type: circle
start: 384
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-199.0, -35.0]
- type: circle
start: 408
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-263.965, -76.2]
# controller 4
@ -206,37 +206,37 @@ led:
start: 432
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [131.982, 76.2]
- type: circle
start: 456
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [131.982, 152.4]
- type: circle
start: 480
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [131.982, 228.6]
- type: circle
start: 504
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [65.991, 266.7]
- type: circle
start: 528
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [65.991, 190.5]
- type: circle
start: 552
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [65.991, 114.3]
@ -245,37 +245,37 @@ led:
start: 576
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [131.982, 0]
- type: circle
start: 600
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [197.973, 38.1]
- type: circle
start: 624
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [197.973, 114.3]
- type: circle
start: 648
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [197.973, 190.5]
- type: circle
start: 672
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [263.965, 152.4]
- type: circle
start: 696
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [263.965, 76.2]
# controller 6
@ -283,37 +283,37 @@ led:
start: 720
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [131.982, -76.2]
- type: circle
start: 744
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [197.973, -38.1]
- type: circle
start: 768
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [263.965, 0]
- type: circle
start: 792
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [263.965, -76.2]
- type: circle
start: 816
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [263.965, -152.4]
- type: circle
start: 840
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [197.973, -114.3]
# controller 7
@ -321,37 +321,37 @@ led:
start: 864
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [65.991, -114.3]
- type: circle
start: 888
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [0, -152.4]
- type: circle
start: 912
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-65.991, -114.3]
- type: circle
start: 936
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-131.982, -76.2]
- type: circle
start: 960
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-197.973, -114.3]
- type: circle
start: 984
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-133.0, -151.0]
# controller 8
@ -359,37 +359,37 @@ led:
start: 1008
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [0, -228.6]
- type: circle
start: 1032
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-65.991, -190.5]
- type: circle
start: 1056
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-65.991, -266.7]
- type: circle
start: 1080
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-131.982, -228.6]
- type: circle
start: 1104
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-197.973, -190.5]
- type: circle
start: 1128
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [-263.965, -152.4]
# controller 9
@ -397,37 +397,37 @@ led:
start: 1152
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [0, -304.8]
- type: circle
start: 1176
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [65.991, -266.7]
- type: circle
start: 1200
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [131.982, -228.6]
- type: circle
start: 1224
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [197.973, -190.5]
- type: circle
start: 1248
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [131.982, -152.4]
- type: circle
start: 1272
size: 24
diameter: 63.5
angle: 0
angle: 180
pos: [65.991, -190.5]
# Strips

6
get_specs.py
View File

@ -336,6 +336,8 @@ def get_multi(partnums, delay, dir, webport, cache=True, bar=None):
elif _download_datasheet(search_result["datasheet"], output_dir) is not False:
out = __downloaded_datasheet(partnum, path, output_dir, dstype, weburl, search_result)
returnval = [partnum, dstype, True, out]
elif os.path.exists(path) and os.path.getsize(path) > 1 and cache:
search_result = {}
@ -357,6 +359,8 @@ def get_multi(partnums, delay, dir, webport, cache=True, bar=None):
# Failed to download with search or guess :(
else:
return False
time.sleep(delay)
actualpartnums.append(returnval)
return returnval
@ -406,7 +410,7 @@ def get_multi(partnums, delay, dir, webport, cache=True, bar=None):
failed.append((partnum, dstype))
# bar(skipped=True)
# bar(skipped=True)
time.sleep(delay)
if len(failed) > 0:
fprint("Failed to download:")

2
jukebox-web

@ -1 +1 @@
Subproject commit ee0ab013550a2374e0afeb7e2070de3b62652422
Subproject commit 3db7609bd541b396b3f4d04533f2ed9e90afe90a

28
led_control.py
View File

@ -95,13 +95,13 @@ class LEDSystem():
for x in range(lednum + shape["size"] - len(self.leds)):
self.leds.append(None)
self.leds_size.append(None)
while angle < 359.999:
while angle > -359.999:
tmpangle = angle + shape["angle"]
x = math.cos(tmpangle * (math.pi / 180.0)) * radius + shape["pos"][1] # flip by 90 degress when we changed layout
y = math.sin(tmpangle * (math.pi / 180.0)) * radius + shape["pos"][0]
self.leds[lednum] = (x,y)
lednum = lednum + 1
angle = angle + anglediv
angle = angle - anglediv
elif shape["type"] == "strip":
angle = shape["angle"]
@ -316,8 +316,8 @@ class LEDSystem():
self.data[x] = val
def setallringsexcept(self, r,g,b, exclude):
startidx1 = self.rings[0][2]
endidx2 = self.rings[-1][3]
startidx1 = 0
endidx2 = len(self.leds)
endidx1 = self.rings[exclude][2]
startidx2 = self.rings[exclude][3]+1
self.setrange(startidx1, endidx1, r,g,b)
@ -410,7 +410,7 @@ class LEDSystem():
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")
self.setmode("Moving")
elif self.mode == "GrabAB":
if self.firstrun:
self.firstrun = False
@ -429,7 +429,7 @@ class LEDSystem():
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)
radius = int(arm_position[2] * 1000) / 3
base = (0,50,100)
target = (100,100,100)
deltar = target[0] - base[0]
@ -440,13 +440,17 @@ class LEDSystem():
exclude = self.rings[ring][2],self.rings[ring][3]
# fade outwards
for idx,led in enumerate(self.leds):
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 posxy[0] - led[0] < radius and posxy[1] - led[1] < radius and idx < exclude[0] or idx > exclude[1]:
dist = math.isqrt(int(math.pow(int(posxy[0] - led[0]), 2) + math.pow(int(posxy[1] - led[1]), 2)))
#dist = max(posxy[0] - led[0], posxy[1] - led[1])
if dist < radius:
ratio = dist/radius
ratio = 1 - dist/radius
#if ratio < 0:
# ratio = 0
self.data[idx] = (int(base[0] + ratio * deltar), int(base[1] + ratio * deltag), 100) #base[2] + ratio * deltab)
elif self.mode == "idle":
self.mode = "Moving"
time.sleep(0)
self.sendall(self.data)
@ -617,11 +621,11 @@ class LEDSystem():
for xx in range(len(leds_normalized2)):
led = leds_normalized2[xx]
x, y = int(round(led[0])), int(round(led[1]))
yinv = image.shape[0]-1
if x < image.shape[1] and y < image.shape[0]:
#avgx += x
#avgy += y
color = tuple(image[y, x])
color = tuple(image[yinv-y, x])
self.setpixel(color[2]/2,color[1]/2,color[0]/2,xx) # swap b & r
#print(color)
else:
@ -722,7 +726,7 @@ if __name__ == "__main__":
ret, frame = cap.read()
if not ret:
break
ledsys.mapimage(frame, fps=120)
ledsys.mapimage(frame, fps=60)
show = False
ring = 1

2
requirements.txt
View File

@ -5,7 +5,7 @@ pypdf2==2.12.1
alive-progress
requests
math3d==4.0.0
git+https://github.com/Byeongdulee/python-urx.git
git+https://git.myitr.org/adeck/python-urx-getl-rt.git
meilisearch
pyyaml
Flask

4
run.py
View File

@ -513,17 +513,19 @@ def mainloop_server(pool, manager):
else:
print("Arm queue message " + str(val))
checkpoint = val
# print(ring_animation, animation_wait, ledsys.mode, arm_position)
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))
ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
led_set_mode = None
animation_wait = 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
else:

34
ur5_control.py
View File

@ -80,7 +80,7 @@ def connect(robot):
while trying and count < 10:
count += 1
try:
robot.robot = urx.Robot(ip)
robot.robot = urx.Robot(ip, use_rt=True)
robot.robot.set_tcp((robot.offset_x, robot.offset_y, robot.offset_z, 0, 0, 0))
# Set weight
robot.robot.set_payload(2, (0, 0, 0.1))
@ -704,12 +704,36 @@ def close_gripper():
def get_position_thread(robot, pos_updates):
robot = connect(robot)
rob = robot.robot
oldvals = rob.getl_rt()
deltavals = [0,0,0]
import uptime
t = 0.01
count = 0
while True:
start = uptime.uptime()
if pos_updates.qsize() < 2:
vals = rob.getl()
pos_updates.put(tuple(vals))
#print("Adding position to queue")
time.sleep(0.01)
vals = rob.getl_rt()
if vals != oldvals:
pos_updates.put(tuple(oldvals))
#time.sleep(0.01)
# deltavals = list()
# deltavals.append(vals[0]-oldvals[0])
# deltavals.append(vals[1]-oldvals[1])
# deltavals.append(vals[2]-oldvals[2])
# count = 0
oldvals = vals
# else:
# count += 0.2
# if count < 1:
# tmpvals = vals
# tmpvals[0] = oldvals[0] + deltavals[0]*count
# tmpvals[1] = oldvals[1] + deltavals[1]*count
# tmpvals[2] = oldvals[2] + deltavals[2]*count
# pos_updates.put(tuple(tmpvals))
while start + t > uptime.uptime():
time.sleep(0.0001)
if __name__ == "__main__":