Add real-time UR5 position updates (hopefully)

config.yml

@@ -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

get_specs.py

@@ -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:")

jukebox-web

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

led_control.py

@@ -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]
+        startidx1 = 0
-        endidx2 = self.rings[-1][3]
+        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]:
+                    if posxy[0] - led[0] < radius and posxy[1] - led[1] < radius and 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)))
+                        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

requirements.txt

@@ -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

run.py

@@ -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:

ur5_control.py

@@ -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()
+            vals = rob.getl_rt()
-            pos_updates.put(tuple(vals))
+            if vals != oldvals:
-            #print("Adding position to queue")
+                pos_updates.put(tuple(oldvals))
-        time.sleep(0.01)
+                #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__":