Add LED animations to runtime

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "jukebox-web"]
+	path = jukebox-web
+	url = https://git.myitr.org/Jukebox/jukebox-web

Dockerfile

@@ -3,11 +3,11 @@ FROM python:3.11-slim
 # Get runtime dependencies
 # glx for OpenCV, ghostscript for datasheet PDF rendering, zbar for barcode scanning, git for cloning repos
 RUN apt-get update && apt-get install -y libgl1-mesa-glx ghostscript libzbar0 git && apt-get clean && rm -rf /var/lib/apt/lists
-COPY *.py *.yml *.sh *.txt *.html static templates ./
+COPY requirements.txt ./
 #COPY config-server.yml config.yml
 RUN pip3 install -r requirements.txt
-
-CMD ["python3", "run.py"]
+COPY *.py *.yml *.sh *.txt *.html static templates ./
+CMD ["sh", "-c", "python3 run.py"]
 EXPOSE 5000
 EXPOSE 8000
 EXPOSE 9000

banner_ivu_export.py

@@ -40,13 +40,16 @@ class DriveImg():
             self.onLine = False 
             fprint("Offline")
 
+    def close(self):
+        self.trans.close()
+
     def read_img(self):
         resposta = 'Falha'
         try:
             if not self.onLine:
                 #print(f'tentando Conectar camera {self.ip}...')
                 gravaLog(ip=self.ip,msg=f'Trying to connect...')
-                sleep(2)
+                #sleep(2)
                 try:
                     self.trans = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
                     self.trans.connect((self.ip,self.PORT))
@@ -63,13 +66,13 @@ class DriveImg():
                 if valida.find("TC IMAGE")<0:
                     self.onLine = False
                     self.trans.close()
-                    sleep(2)
+                    #sleep(2)
                     gravaLog(ip=self.ip,tipo="Falha",msg=f'Unable to find TC IMAGE bookmark')
                     return "Error"
             except Exception as ex:
                 self.onLine = False
                 self.trans.close()
-                sleep(2)
+                #sleep(2)
                 gravaLog(ip=self.ip,tipo="Falha",msg=f'Error - {str(ex)}')
                 return "Error"
             if ret:
@@ -113,7 +116,7 @@ class DriveImg():
             #print(f'erro {str(ex)}')
             self.onLine = False
             self.trans.close()
-            sleep(2)
+            #sleep(2)
             return resposta
 
 class DriveData():
@@ -139,7 +142,7 @@ class DriveData():
             if not self.onLine:
                 #print(f'tentando Conectar...\n')
                 gravaLog(ip=self.ip,msg=f'tentando Conectar...',file="log_data.txt")
-                sleep(2)
+                #sleep(2)
                 try:
                     self.trans = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
                     self.trans.connect((self.ip,self.PORT))
@@ -154,7 +157,7 @@ class DriveData():
         except Exception as ex:
             self.onLine = False 
             gravaLog(ip=self.ip,tipo="Falha Generica",msg=f'erro {str(ex)}',file="log_data.txt")
-            sleep(2)
+            #sleep(2)
             return resposta
 
 

compose-search-only.yml

@@ -0,0 +1,13 @@
+services:
+  meilisearch:
+    image: "getmeili/meilisearch:v1.6.2"
+    ports:
+      - "7700:7700"
+    environment:
+      MEILI_MASTER_KEY: fluffybunnyrabbit
+      MEILI_NO_ANALYTICS: true
+    volumes:
+      - "meili_data:/meili_data"
+  
+volumes:
+  meili_data:

compose.yml

@@ -8,6 +8,23 @@ services:
       MEILI_NO_ANALYTICS: true
     volumes:
       - "meili_data:/meili_data"
+  
+  jukebox-software:
+    build: .
+    init: true
+    ports:
+      - "5000:5000"
+      - "8000:8000"
+      - "9000:9000"
+    environment:
+      - PYTHONUNBUFFERED=1
+    depends_on:
+      - meilisearch
+  
+  jukebox-web:
+    build: jukebox-web
+    ports:
+      - "3000:3000"
 
 volumes:
   meili_data:

config.yml

@@ -3,9 +3,21 @@ core:
   serverip: 172.26.178.114
   clientip: 172.26.176.1
   server: Hyper-Vd
+  loopspeed: 60 # fps
 
 arm:
   ip: 192.168.1.145
+  tool:
+    offset_x: 0
+    offset_y: 0
+    offset_z: 0.14
+  limbs:
+    limb_base: 0.11
+    limb1: 0.425
+    limb2: 0.39225
+    limb3: 0.1
+    limb_wrist: 0.0997
+
 
 #cable_map:
 cameras:
@@ -15,55 +27,55 @@ cameras:
 
 led: 
   fps: 90
-  timeout: 0
+  timeout: 1
   controllers:
-    - universe: 9
-      ip: 192.168.68.131
+    - universe: 0
+      ip: 192.168.1.200
       ledstart: 0
       ledend: 143
       mode: rgb
-    - universe: 3
-      ip: 192.168.68.131
+    - universe: 1
+      ip: 192.168.1.201
       ledstart: 144
       ledend: 287
       mode: rgb
     - universe: 2
-      ip: 192.168.68.131
+      ip: 192.168.1.202
       ledstart: 288
       ledend: 431
       mode: rgb
-    - universe: 4
-      ip: 192.168.5.40
+    - universe: 3
+      ip: 192.168.1.203
       ledstart: 432
       ledend: 575
       mode: rgb
-    - universe: 1
-      ip: 192.168.5.4
+    - universe: 4
+      ip: 192.168.1.204
       ledstart: 576
       ledend: 719
       mode: rgb
     - universe: 5
-      ip: 192.168.68.131
+      ip: 192.168.1.205
       ledstart: 720
       ledend: 863
       mode: rgb
     - universe: 6
-      ip: 192.168.68.131
+      ip: 192.168.1.206
       ledstart: 864
       ledend: 1007
       mode: rgb
     - universe: 7
-      ip: 192.168.68.131
+      ip: 192.168.1.207
       ledstart: 1008
       ledend: 1151
       mode: rgb
     - universe: 8
-      ip: 192.168.68.131
+      ip: 192.168.1.208
       ledstart: 1152
       ledend: 1295
       mode: rgb
-    - universe: 0
-      ip: 192.168.68.130
+    - universe: 9
+      ip: 192.168.1.209
       ledstart: 1296
       ledend: 1365
       mode: rgbw

get_specs.py

@@ -159,8 +159,8 @@ def touch(path):
 
 
 
-def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
-    with alive_bar(len(partnums) * 2, dual_line=True, calibrate=30, bar="classic2", spinner="classic") as bar:
+def get_multi(partnums, delay=0.25, dir="cables/", cache=True, bar=None):
+    #with alive_bar(len(partnums) * 2, dual_line=True, calibrate=30, bar="classic2", spinner="classic", disable=True, file=sys.stdout) as bar:
         failed = list()
         actualpartnums = list()
         def _try_download_datasheet(partnum, output_dir, dstype): # Guess datasheet URL
@@ -188,7 +188,7 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
                             # and set chunk_size parameter to None.
                             #if chunk: 
                             bartext = bartext + "."
-                            bar.text = bartext
+                            # bar.text = bartext
                             f.write(chunk)
                 #fprint("")
                 return output_dir + "/datasheet.pdf"
@@ -217,7 +217,7 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
                             # and set chunk_size parameter to None.
                             #if chunk: 
                             bartext = bartext + "."
-                            bar.text = bartext
+                            # bar.text = bartext
                             f.write(chunk)
                 #fprint("")
                 return output_dir + "/datasheet.pdf"
@@ -244,7 +244,7 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
                             # and set chunk_size parameter to None.
                             #if chunk: 
                             bartext = bartext + "."
-                            bar.text = bartext
+                            # bar.text = bartext
                             f.write(chunk)
                 #fprint("")
                 return output_dir + "/part-hires." + url.split(".")[-1]
@@ -255,29 +255,29 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
 
         def __use_cached_datasheet(partnum, path, output_dir, dstype):
             fprint("Using cached datasheet for " + partnum)
-            bar.text = "Using cached datasheet for " + partnum
-            bar(skipped=True)
+            # bar.text = "Using cached datasheet for " + partnum
+            # bar(skipped=True)
             if not os.path.exists(output_dir + "/parsed"):
                 
                 fprint("Parsing Datasheet contents of " + partnum)
-                bar.text = "Parsing Datasheet contents of " + partnum + ".pdf..."
+                # bar.text = "Parsing Datasheet contents of " + partnum + ".pdf..."
             
                 out = read_datasheet.parse(path, output_dir, partnum, dstype)
-                bar(skipped=False) 
+                # bar(skipped=False) 
                 return out
             else:
                 fprint("Datasheet already parsed for " + partnum)
-                bar.text = "Datasheet already parsed for " + partnum + ".pdf"
-                bar(skipped=True)
+                # bar.text = "Datasheet already parsed for " + partnum + ".pdf"
+                # bar(skipped=True)
 
         def __downloaded_datasheet(partnum, path, output_dir, dstype):
             fprint("Downloaded " + path)
-            bar.text = "Downloaded " + path
-            bar(skipped=False)
+            # bar.text = "Downloaded " + path
+            # bar(skipped=False)
             fprint("Parsing Datasheet contents of " + partnum)
-            bar.text = "Parsing Datasheet contents of " + partnum + ".pdf..."
+            # bar.text = "Parsing Datasheet contents of " + partnum + ".pdf..."
             out = read_datasheet.parse(path, output_dir, partnum, dstype)
-            bar(skipped=False)
+            # bar(skipped=False)
             return out
 
         def run_search(partnum):
@@ -290,7 +290,7 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
             output_dir = dir + partnum
             path = output_dir + "/datasheet.pdf"
             bartext = "Downloading files for part " + partnum
-            bar.text = bartext
+            # bar.text = bartext
             partnum = oldpartnum.replace("_","/")
             returnval = [partnum, dstype, False, False]
             if (not os.path.exists(output_dir + "/found_part_hires")) or not (os.path.exists(path) and os.path.getsize(path) > 1) or not cache:
@@ -305,7 +305,7 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
                     output_dir = dir + partnum
                     path = output_dir + "/datasheet.pdf"
                     bartext = "Downloading files for part " + partnum
-                    bar.text = bartext
+                    # bar.text = bartext
 
                     if not os.path.exists(output_dir + "/found_part_hires") or not cache:
                         if _download_image(search_result["image"], output_dir):
@@ -344,9 +344,13 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
                 fprint("Using cached hi-res part image for " + partnum)
                 out = __use_cached_datasheet(partnum, path, output_dir, dstype)
                 returnval = [partnum, dstype, False, out]
+                actualpartnums.append(returnval)
                 return True
     
         for fullpartnum in partnums:
+            if fullpartnum is False:
+                actualpartnums.append(False)
+                continue
             if fullpartnum[0:2] == "BL": # catalog.belden.com entry
                 partnum = fullpartnum[2:]
                 dstype = "Belden"
@@ -373,19 +377,19 @@ def get_multi(partnums, delay=0.25, dir="cables/", cache=True):
                             time.sleep(delay)
                 if not success:
                     fprint("Failed to download datasheet for part " + partnum)
-                    bar.text = "Failed to download datasheet for part " + partnum
+                    # bar.text = "Failed to download datasheet for part " + partnum
                     failed.append((partnum, dstype))
-                    bar(skipped=True)
-                    bar(skipped=True)
+                    # bar(skipped=True)
+                    # bar(skipped=True)
             time.sleep(delay)
             
-    if len(failed) > 0:
-        fprint("Failed to download:")
-        for partnum in failed:
-            fprint(partnum[1] + " " + partnum[0])
-        return False, actualpartnums # Go to manual review upload page
-    else:
-        return True, actualpartnums # All cables downloaded; we are good to go
+        if len(failed) > 0:
+            fprint("Failed to download:")
+            for partnum in failed:
+                fprint(partnum[1] + " " + partnum[0])
+            return False, actualpartnums # Go to manual review upload page
+        else:
+            return True, actualpartnums # All cables downloaded; we are good to go
 
 
 
@@ -408,7 +412,7 @@ if __name__ == "__main__":
     # ]
     partnums = [
     # Actual cables in Jukebox
-     
+    "BL3092A",
     "AW86104CY",
     "AW3050",
     "AW6714",
@@ -438,8 +442,9 @@ if __name__ == "__main__":
     "BL6300FE 009Q",
     "BLRA500P 006Q",
 
-
+    ]
     # Some ones I picked, including some invalid ones
+    a = [
     "BL10GXS12", 
     "BLRST%205L-RKT%205L-949",
     "BL10GXS13",
@@ -455,7 +460,7 @@ if __name__ == "__main__":
     "BLFISX006W0", # datasheet only
     "BLFISX00103", # invalid
     "BLC6D1100007" # invalid
-
+    
     ]
     #print(query_search("TT-SLG-024-HTNN", "Belden"))
     from label_generator import gen_label

process_video.py

@@ -8,13 +8,18 @@ from util import fprint
 class qr_reader():
     camera = None
     def __init__(self, ip, port):
-        self.camera = banner_ivu_export.DriveImg(ip, port)
+        self.ip = ip
+        self.port = port
+        #self.camera = banner_ivu_export.DriveImg(ip, port)
 
     def read_qr(self, tries=1):
         print("Trying " + str(tries) + " frames.")
+        self.camera = banner_ivu_export.DriveImg(self.ip, self.port)
         for x in range(tries):
-            try:
-                imgtype, img = self.camera.read_img()
+            print(str(x) + " ", end="", flush=True)
+            imgtype, img = self.camera.read_img()
+                
+            if True:    
                 #fprint(imgtype)
                 image_array = np.frombuffer(img, np.uint8)
                 img = cv2.imdecode(image_array, cv2.IMREAD_COLOR)
@@ -22,9 +27,13 @@ class qr_reader():
                 #cv2.waitKey(1)
                 detect = cv2.QRCodeDetector()
                 value, points, straight_qrcode = detect.detectAndDecode(img)
-                return value
-            except:
-                continue
+                
+                if value != "":
+                    self.camera.close()
+                    return value
+                    
+
+        self.camera.close()
         return False
         
 
@@ -41,5 +50,7 @@ class video_streamer():
 
 if __name__ == "__main__":
     test = qr_reader("192.168.1.125", 32200)
+    import time
     while True:
-        fprint(test.read_qr(5))
+        fprint(test.read_qr(300))
+        time.sleep(1)

read_datasheet.py

@@ -177,7 +177,7 @@ def parse(filename, output_dir, partnum, dstype):
             if dstype == "Alphawire" and table_name_2.find("\n") >= 0:
                 torename[table_name_2] = table_name_2[0:table_name_2.find("\n")]
 
-            if table_name_2.find(table.iloc[-1, 0]) >= 0:
+            if dstype == "Alphawire" and table_name_2.find(table.iloc[-1, 0]) >= 0:
                 # Name taken from table directly above - this table does not have a name
                 torename[table_name_2] = "Specs " + str(len(tables))
                 #table_list["Specs " + str(len(tables))] = table_list[table_name_2] # rename table to arbitrary altername name
@@ -251,9 +251,6 @@ def parse(filename, output_dir, partnum, dstype):
                 #fprint(table_name)
                 #fprint(previous_table)
                 
-                
-                
-                
                 main_key = previous_table
                 cont_key = table_name
                 #fprint(tables)
@@ -267,15 +264,21 @@ def parse(filename, output_dir, partnum, dstype):
                     del tables[table_name]
     
                 else:
+                    #print(tables)
+                    #print(main_key)
+                    #print(cont_key)
                     for key in tables[cont_key].keys():
                         tables[main_key][key] = tables[cont_key][key]
                     del tables[table_name]
-
-        previous_table = table_name
+            else:
+                previous_table = table_name
+        else:
+            previous_table = table_name
     
     # remove & rename tables
+    #print(torename)
     for table_name in torename.keys():
-        tables[torename[table_name]] = tables[table_name]
+        tables[torename[str(table_name)]] = tables[str(table_name)]
         del tables[table_name]
     # remove multi-line values that occasionally squeak through
     def replace_newlines_in_dict(d):
@@ -298,7 +301,7 @@ def parse(filename, output_dir, partnum, dstype):
     output_table["id"] = id
     #output_table["position"] = id
     #output_table["brand"] = brand
-    output_table["fullspecs"] = tables
+    output_table["fullspecs"] = {"partnum": partnum, "id": id, **tables}
     output_table["searchspecs"] = {"partnum": partnum, **flatten(tables)}
     
     output_table["searchspecs"]["id"] = id
@@ -313,9 +316,9 @@ def parse(filename, output_dir, partnum, dstype):
     for file_path in json_files:
         os.remove(file_path)
         #print(f"Deleted {file_path}")
-    with open(output_dir + "/search_" + output_table["searchspecs"]["id"] + ".json", 'w') as json_file:
+    with open(output_dir + "/search.json", 'w') as json_file:
         json.dump(output_table["searchspecs"], json_file)
-    with open(output_dir + "/specs_" + output_table["partnum"] + ".json", 'w') as json_file:
+    with open(output_dir + "/specs.json", 'w') as json_file:
         json.dump(output_table["fullspecs"], json_file)
 
     #print(json.dumps(output_table, indent=2))
@@ -346,12 +349,20 @@ def flatten(tables):
 
             fullkeyname = (table + ": " + keyname).replace(".","")
             if type(tables[table][key]) is not tuple:
-                out[fullkeyname] = convert_to_number(tables[table][key])
+                if len(tables[table][key]) > 0:
+                    out[fullkeyname] = convert_to_number(tables[table][key])
                 #print("\"" + keyname + "\":", "\"" + str(out[fullkeyname]) + "\",")
             elif len(tables[table][key]) == 1:
-                out[fullkeyname] = convert_to_number(tables[table][key][0])
+                if len(tables[table][key][0]) > 0:
+                    out[fullkeyname] = convert_to_number(tables[table][key][0])
                 #print("\"" + keyname + "\":", "\"" + str(out[fullkeyname]) + "\",")
-
+            else:
+                tmp = []
+                for x in range(len(tables[table][key])):
+                    if len(tables[table][key][x]) > 0:
+                        tmp.append(tables[table][key][x].strip())
+                        #out[fullkeyname + " " + str(x+1)] = convert_to_number(tables[table][key][x])
+                out[fullkeyname] = tmp
             # if the item has at least two commas in it, split it
             if tables[table][key].count(',') > 0:
                 out[fullkeyname] = list(map(lambda x: x.strip(), tables[table][key].split(",")))

requirements.txt

@@ -4,6 +4,7 @@ opencv-python
 pypdf2==2.12.1
 alive-progress
 requests
+math3d==4.0.0
 git+https://github.com/Byeongdulee/python-urx.git
 meilisearch
 pyyaml
@@ -20,6 +21,7 @@ pyarrow
 ghostscript
 pyzbar
 segno
+pyModbusTCP
 
 # Development
 matplotlib

run.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 
+from alive_progress import alive_bar
 import get_specs
 import traceback
 #import logging
@@ -12,19 +13,25 @@ from util import fprint
 from util import run_cmd
 import sys
 import ur5_control
+from ur5_control import Rob
 import os
 import signal
 import socket
 from flask import Flask, render_template, request
 import requests
-import led_control
+from led_control import LEDSystem
 import server
 import asyncio
 import json
 import process_video
+import search
+from search import JukeboxSearch
+#multiprocessing.set_start_method('spawn', True)
+from pyModbusTCP.client import ModbusClient
+from uptime import uptime
 
 
-
+mbconn = None
 config = None
 keeprunning = True
 arm_ready = False
@@ -32,25 +39,45 @@ led_ready = False
 camera_ready = False
 sensor_ready = False
 vm_ready = False
+cable_search_ready = False
 killme = None
 #pool = None
 serverproc = None
 camera = None
-
+ledsys = None
+arm = None
 to_server_queue = Queue()
 from_server_queue = Queue()
+mainloop_get = Queue()
+mode = "Startup"
+counter = 0
+jbs = None
+scan_value = None
+arm_state = None
+cable_list = list()
+parse_res = None
+cable_list_state = list()
+just_placed = -1
+ring_animation = None
+led_set_mode = None
 
 def arm_start_callback(res):
+    fprint("Arm action complete.")
     global arm_ready
     arm_ready = True
+    
 
 def led_start_callback(res):
     global led_ready
     led_ready = True
+    global ledsys
+    ledsys = res
 
 def camera_start_callback(res):
     global camera_ready
     camera_ready = True
+    global scan_value
+    scan_value = res
     
 def sensor_start_callback(res):
     global sensor_ready
@@ -60,6 +87,12 @@ def vm_start_callback(res):
     global vm_ready
     vm_ready = True
 
+def cable_search_callback(res):
+    global cable_search_ready
+    cable_search_ready = True
+    global parse_res 
+    parse_res = res
+
 def wait_for(val, name):
     #global val
     if val is False:
@@ -67,7 +100,15 @@ def wait_for(val, name):
         while val is False:
             sleep(0.1)
 
-def start_server_socket():
+def send_data(type, call, data, client_id="*"):
+    out = dict()
+    out["type"] = type
+    out["call"] = call
+    out["data"] = data
+    to_server_queue.put((client_id, json.dumps(out)))
+
+def start_server_socket(cable_list):
+    global jbs
     """app = Flask(__name__)
 
     @app.route('/report_ip', methods=['POST'])
@@ -99,69 +140,92 @@ def start_server_socket():
             # Message handler
             try:
                 decoded = json.loads(message)
-                if "type" not in decoded:
-                    fprint("Missing \"type\" field.")
-                    continue
-                if "call" not in decoded:
-                    fprint("Missing \"call\" field.")
-                    continue
-                if "data" not in decoded:
-                    fprint("Missing \"data\" field.")
-                    continue
+            except:
+                fprint("Non-JSON message recieved")
+                continue
 
-                # if we get here, we have a "valid" data packet
-                data = decoded["data"]
-                call = decoded["call"]
+            if "type" not in decoded:
+                fprint("Missing \"type\" field.")
+                continue
+            if "call" not in decoded:
+                fprint("Missing \"call\" field.")
+                continue
+            if "data" not in decoded:
+                fprint("Missing \"data\" field.")
+                continue
+            # if we get here, we have a "valid" data packet
+            data = decoded["data"]
+            call = decoded["call"]
+            try:
                 match decoded["type"]:
                     case "log":
                         fprint("log message")
                         if call == "send":
                             fprint("webapp: " + str(data), sendqueue=to_server_queue)
                         elif call == "request":
-                            fprint("")
-
+                            pass
                     case "cable_map":
                         fprint("cable_map message")
                         if call == "send":
-                            fprint("")
+                            pass
                         elif call == "request":
-                            fprint("")
-
+                            tmp = list()
+                            for idx in range(len(cable_list)):
+                                if cable_list[idx] is not False:
+                                    tmp1 = {"part_number": cable_list[idx], "position": idx, "name": cable_list[idx], "brand": "Belden", "description": "Blah", "short_description": "Bla"}
+                                    tmp.append(tmp1)
+                            out = {"map": tmp}
+                            fprint(out)
+                            send_data(decoded["type"], "send", out, client_id)
 
                     case "ping":
                         fprint("Pong!!!")
-
                     # Lucas' notes
                     # Add a ping pong :) response/handler
                     # Add a get cable response/handler
                     #       this will tell the robot arm to move
                     # Call for turning off everything
-
                     # TODO Helper for converting Python Dictionaries to JSON
                     # make function: pythonData --> { { "type": "...", "call": "...", "data": pythonData } }
-                            
+
                     # to send: to_server_queue.put(("*", "JSON STRING HERE")) # replace * with UUID of client to send to one specific location
-                    
+
                     case "cable_details":
                         fprint("cable_details message")
                         if call == "send":
-                            fprint("")
+                            pass
                         elif call == "request":
-                            fprint("")
+                            dataout = dict()
+                            dataout["cables"] = list()
+                            print(data)
+                            if "part_number" in data:
+                                for part in data["part_number"]:
+                                    #print(part)
+                                    #print(jbs.get_partnum(part))
+                                    dataout["cables"].append(jbs.get_partnum(part)["fullspecs"])
+                            if "position" in data:
+                                for pos in data["position"]:
+                                    #print(pos)
+                                    #print(jbs.get_position(str(pos)))
+                                    dataout["cables"].append(jbs.get_position(str(pos))["fullspecs"])
+                            send_data(decoded["type"], "send", dataout, client_id)
 
                     case "cable_search":
                         fprint("cable_search message")
                         if call == "send":
-                            fprint("")
+                            pass
                         elif call == "request":
-                            fprint("")
-
+                            results = jbs.search(data["string"])["hits"]
+                            dataout = dict()
+                            dataout["cables"] = list()
+                            for result in results:
+                                dataout["cables"].append(result["fullspecs"])
+                            send_data(decoded["type"], "send", dataout, client_id)
                     case "keyboard":
                         fprint("keyboard message")
                         if call == "send":
-                            fprint("")
+                            pass
                         elif call == "request":
-                            fprint("")
                             if data["enabled"] == True:
                                 # todo : send this to client
                                 p = Process(target=run_cmd, args=("./keyboard-up.ps1",))
@@ -169,21 +233,32 @@ def start_server_socket():
                             elif data["enabled"] == False:
                                 p = Process(target=run_cmd, args=("./keyboard-down.ps1",))
                                 p.start()
-
                     case "machine_settings":
                         fprint("machine_settings message")
                         if call == "send":
-                            fprint("")
+                            pass
                         elif call == "request":
-                            fprint("")
+                            pass
 
+                    case "cable_get":
+                        fprint("cable_get message")
+                        if call == "send":
+                            global mainloop_get
+                            if "part_number" in data:
+                                for cableidx in range(len(cable_list)):
+                                    cable = cable_list[cableidx]
+                                    if cable == data["part_number"]:
+                                        mainloop_get.put(("pickup", cableidx))
+                            elif "position" in data:
+                                mainloop_get.put(("pickup", data["position"]))
+                    
                     case _:
                         fprint("Unknown/unimplemented data type: " + decoded["type"])
+            except Exception as e:
+                fprint(traceback.format_exc())
+                fprint(e)
 
-
-            except:
-                fprint("Non-JSON message recieved")
-                continue
+            
 
 
         sleep(0.001)  # Sleep to prevent tight loop
@@ -231,57 +306,352 @@ def setup_server(pool):
     global arm_ready
     global serverproc
     global camera
+    global arm
+    global jbs
 
-    pool.apply_async(ur5_control.init, (config["arm"]["ip"],), callback=arm_start_callback)
-    pool.apply_async(led_control.init, callback=led_start_callback)
+    arm = Rob(config)
+    pool.apply_async(ur5_control.powerup_arm, (arm,), callback=arm_start_callback, error_callback=handle_error)
+    global ledsys
+    ledsys = LEDSystem()
+    #pool.apply_async(ledsys.init, callback=led_start_callback)
     #pool.apply_async(sensor_control.init, callback=sensor_start_callback)
-    serverproc = Process(target=start_server_socket)
-    serverproc.start()
-
+    jbs = JukeboxSearch()
+    
+    
+    
     if led_ready is False:
         fprint("waiting for " + "LED controller initialization" + " to complete...", sendqueue=to_server_queue)
-        while led_ready is False:
-            sleep(0.1)
+        ledsys.init()
+        led_ready = True
     fprint("LED controllers initialized.", sendqueue=to_server_queue)
-    #to_server_queue.put("[log] LED controllers initialized.")
-    sensor_ready = True
+
     if sensor_ready is False:
         fprint("waiting for " + "Sensor Initialization" + " to complete...", sendqueue=to_server_queue)
-        while sensor_ready is False:
-            sleep(0.1)
+        global mbconn
+        mbconn = ModbusClient(host="localhost", port=502, unit_id=1, auto_open=True, auto_close=True)
     fprint("Sensors initialized.", sendqueue=to_server_queue)
 
     if camera_ready is False:
         fprint("waiting for " + "Camera initilization" + " to complete...", sendqueue=to_server_queue)
-        # camera = process_video.qr_reader(config["cameras"]["banner"]["ip"], config["cameras"]["banner"]["port"])
+        camera = process_video.qr_reader(config["cameras"]["banner"]["ip"], int(config["cameras"]["banner"]["port"]))
 
     fprint("Camera initialized.", sendqueue=to_server_queue)
 
-    arm_ready = True
+    #arm_ready = True
     if arm_ready is False:
-        fprint("waiting for " + "UR5 initilization" + " to complete...", sendqueue=to_server_queue)
+        fprint("waiting for " + "UR5 powerup" + " to complete...", sendqueue=to_server_queue)
         while arm_ready is False:
             sleep(0.1)
+
+    ur5_control.init_arm(arm)
     fprint("Arm initialized.", sendqueue=to_server_queue)
+
+
     
-    
-    
-        
     return True
 
+def handle_error(error):
+	print(error, flush=True)
 
+def get_sensors():
+    global mbconn
+    global sensors
+    oldsens = sensors
+    """
+    port 1: 256
+    port 2: 272
+    port 3: 288
+    port 4: 304
+    port 5: 320
+    port 6: 336
+    port 7: 352
+    port 8: 368
+    
+    """
+    out = list()
+    for reg in [352, 288, 304, 368]:
+        val = mbconn.read_holding_registers(reg)
+        if val == 1:
+            out.append(1)
+        else:
+            out.append(0)
+
+    sensors = out
+
+    for x in range(len(oldsens)):
+        if oldsens[x] == 0 and out[x] == 1:
+            # cable newly detected on tray
+            return x
+        
+    return -1
+def get_open_spot(sensordata):
+    for x in range(len(sensordata)):
+        sens = sensordata[x]
+        if not sens:
+            return x
+        
+    # if we get here, every spot is full
+        
+    return False
+    
 
 def mainloop_server(pool):
+    # NON-blocking loop
+    global ring_animation
+    global led_set_mode
+    global just_placed
     global config
     global counter
     global killme
+    global mode
+    global jbs
+    global arm
+    global ledsys
+    global camera
+    global arm_ready
+    global arm_state
+    global camera_ready
+    global cable_search_ready
+    global cable_list
+    global mainloop_get
+    global cable_list_state
 
     if killme.value > 0:
         killall()
-    counter = counter + 1
 
-    # fprint("Looking for QR code...")
-    # print(camera.read_qr(30))
+    if mode == "Startup":
+        #counter = 54 # remove for demo
+        if counter < 54:
+            # scanning cables
+            ring_animation = counter
+            led_set_mode = "GrabA"
+            if arm_state is None:
+                #pool.apply_async(arm_start_callback, ("",))
+                arm_ready = False
+                pool.apply_async(ur5_control.to_camera, (arm,counter), callback=arm_start_callback, error_callback=handle_error)
+                fprint("Getting cable index " + str(counter) + " and scanning...")
+                arm_state = "GET"
+                #ur5_control.to_camera(arm, counter)
+                #arm_ready = True
+
+            elif arm_ready and arm_state == "GET":
+                fprint("Looking for QR code...")
+                pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
+                arm_ready = False
+
+            elif camera_ready:
+                fprint("Adding cable to list...")
+                global scan_value
+                if scan_value is False:
+                    cable_list.append(scan_value)
+                elif scan_value.find("bldn.app/") > -1:
+                    scan_value = scan_value[scan_value.find("bldn.app/")+9:]
+                else:
+                    cable_list.append(scan_value)
+                fprint(scan_value)
+                pool.apply_async(ur5_control.return_camera, (arm,counter), callback=arm_start_callback, error_callback=handle_error)
+                #ur5_control.return_camera(arm, counter)
+                #arm_ready = True
+                arm_state = "RETURN"
+                camera_ready = False
+
+            elif arm_ready and arm_state == "RETURN":
+                counter += 1
+                arm_state = None
+            else:
+                # just wait til arm/camera is ready
+                pass
+        else:
+            # scanned everything
+            ring_animation = None
+            led_set_mode == "idle"
+            tmp = [
+                    # Actual cables in Jukebox
+                    "BLTF-1LF-006-RS5",
+                    "BLTF-SD9-006-RI5",
+                    "BLTT-SLG-024-HTN",
+                    "BLFISX012W0",
+                    "BLFI4X012W0",
+                    "BLSPE101",
+                    "BLSPE102",
+                    "BL7922A",
+                    "BL7958A",
+                    "BLIOP6U",
+                    "BL10GXW13",
+                    "BL10GXW53",
+                    "BL29501F",
+                    "BL29512",
+                    "BL3106A",
+                    "BL9841",
+                    "BL3105A",
+                    "BL3092A",
+                    "BL8760",
+                    "BL6300UE",
+                    "BL6300FE",
+                    "BLRA500P",
+                    "AW86104CY",
+                    "AW3050",
+                    "AW6714",
+                    "AW1172C",
+                    "AWFIT-221-1_4"
+                ]
+            while len(tmp) < 54:
+                tmp.append(False) # must have 54 entries
+
+            #cable_list = tmp # comment out for real demo
+                
+            for idx in range(len(cable_list)):
+                cable_list_state.append(True)
+
+            pool.apply_async(get_specs.get_multi, (cable_list, 0.3), callback=cable_search_callback, error_callback=handle_error)
+            mode = "Parsing"
+            fprint("All cables scanned. Finding & parsing datasheets...")
+    if mode == "Parsing":
+            # waiting for search & parse to complete
+            #cable_search_ready = True
+            if cable_search_ready is False:
+                pass
+            else:
+                # done
+                global parse_res
+                success, partnums = parse_res
+                for idx in range(len(partnums)):
+                    if partnums[idx] is not False:
+                        cable_list[idx] = partnums[idx][0].replace("/", "_")
+                    else:
+                        cable_list[idx] = False
+                
+                print(partnums)
+                if success:
+                    # easy mode
+                    fprint("All cables inventoried and parsed.")
+                    fprint("Adding to database...")
+                    for idx in range(len(cable_list)):
+                        partnum = cable_list[idx]
+                        if partnum is not False:
+                            with open("cables/" + partnum + "/search.json", "rb") as f:
+                                searchdata = json.load(f)
+                                searchdata["position"] = idx
+                            with open("cables/" + partnum + "/specs.json", "rb") as f:
+                                specs = json.load(f)
+                            searchdata["fullspecs"] = specs
+                            searchdata["fullspecs"]["position"] = idx
+                            jbs.add_document(searchdata)
+                    #sleep(0.5)
+                    #print(jbs.get_position("1"))
+                    
+                    fprint("All cables added to database.")
+                    mode = "Idle"
+                    serverproc = Process(target=start_server_socket, args=(cable_list,), error_callback=handle_error)
+                    serverproc.start()
+                else:
+                    # TODO: manual input
+                    pass
+
+            
+    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
+
+        else:
+            global mainloop_get
+            newtube = get_sensors()
+            if newtube >= 0 and newtube != just_placed:
+                # need to return a cable
+                mainloop_get.put(("return", newtube))
+                just_placed = -1
+            
+            if not mainloop_get.empty():
+                action, get_cable = mainloop_get.get()
+                if get_cable > -1:
+                    global sensors
+                    if action == "pickup":
+                        spot = get_open_spot(sensors)
+                        if spot is not False:
+                            arm_ready = False
+                            pool.apply_async(ur5_control.pick_up_routine, (arm, get_cable, True, spot), callback=arm_start_callback, error_callback=handle_error)
+                            mode = "Pickup"
+                            cable_list_state[get_cable] = False # mark as removed
+                            get_sensors(sensors,flag=get_open_spot(sensors))
+
+                    if action == "return":
+                        arm_ready = False
+                        pool.apply_async(ur5_control.return_routine, (arm, get_cable), callback=arm_start_callback, error_callback=handle_error)
+                        mode = "ReturnC"
+            else:
+                # LED idle anim
+                pass
+
+    if mode == "Pickup":
+        # complete
+        if arm_ready == True:
+            mode = "Idle"
+            arm_ready = False
+            
+        else:
+            # getting cable and bringing to tray
+            # led animation
+            pass
+    
+    if mode == "ReturnC":
+        # complete
+        if arm_ready == True:
+            mode = "Scan"
+            arm_ready = False
+            camera_ready = False
+            pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
+            
+        else:
+            # getting cable from and bringing to camera
+            # led animation
+            pass
+
+    if mode == "Scan":
+        if camera_ready == True:
+            global scan_value
+            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
+            elif scan_value.find("bldn.app/") > -1:
+                scan_value = scan_value[scan_value.find("bldn.app/")+9:]
+            
+            fprint("Got cable: " + scan_value)
+
+            for idx in range(len(cable_list)):
+                cable = cable_list[idx]
+                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.return_camera, (arm, idx), callback=arm_start_callback, error_callback=handle_error)
+                    mode = "Return"
+                    break
+
+            
+
+    if mode == "Return":
+        if arm_ready == True:
+            mode = "Idle"
+            arm_ready = False
+            # movement finished
+
+        else: 
+            # cable going from camera to holder
+            # led animation
+            pass
+            
+    if True:
+        # do every loop!
+        if ring_animation is not None and ledsys.mode != "idle":
+            ledsys.mainloop(None, ring_animation)
+        elif ring_animation is not None:
+            ledsys.mainloop(led_set_mode, ring_animation)
+        
+
 
 def run_loading_app():
     
@@ -389,19 +759,20 @@ class LogExceptions(object):
         return result
 
 class LoggingPool(Pool):
-    def apply_async(self, func, args=(), kwds={}, callback=None):
-        return Pool.apply_async(self, LogExceptions(func), args, kwds, callback)
+    def apply_async(self, func, args=(), kwds={}, callback=None, error_callback=None):
+        return Pool.apply_async(self, LogExceptions(func), args, kwds, callback, error_callback)
 
 
 if __name__ == "__main__":
     #sys.stdout = Logger(filename="output.log")
     #sys.stderr = Logger(filename="output.log")
     #log_to_stderr(logging.DEBUG)
+    
     fprint("Starting Jukebox control system...")
     with open('config.yml', 'r') as fileread:
         #global config
         config = yaml.safe_load(fileread)
-
+    fprint("Config loaded.")
     with Manager() as manager:
         fprint("Spawning threads...")
         pool = LoggingPool(processes=10)
@@ -422,5 +793,18 @@ if __name__ == "__main__":
                 fprint("Entering main loop...")
                 while(keeprunning):
                     mainloop_server(pool)
+        else:
+            fprint("Mode unspecified - assuming server")
+            fprint("Starting in server mode.")
+            if setup_server(pool):
+                fprint("Entering main loop...")
+                start = 0
+                speed = config["loopspeed"]
+                while(keeprunning):
+                    start = uptime()
+                    mainloop_server(pool)
+                    # limit to certain "framerate"
+                    while start + 1.0/speed < uptime():
+                        pass
 
 

search.py

@@ -1,10 +1,12 @@
+#!/usr/bin/env python3
+
 """Interactions with the Meilisearch API for adding and searching cables."""
 from meilisearch import Client
 from meilisearch.task import TaskInfo
 from meilisearch.errors import MeilisearchApiError
-import json
+import time
 
-DEFAULT_URL = "http://localhost:7700"
+DEFAULT_URL = "http://127.0.0.1:7700"
 DEFAULT_APIKEY = "fluffybunnyrabbit" # I WOULD RECOMMEND SOMETHING MORE SECURE
 DEFAULT_INDEX = "cables"
 DEFAULT_FILTERABLE_ATTRS = ["partnum", "uuid", "position"] # default filterable attributes
@@ -34,12 +36,15 @@ class JukeboxSearch:
         # create the index if it does not exist already
         try:
             self.client.get_index(self.index)
+            self.client.delete_index(self.index)
+            self.client.create_index(self.index)
         except MeilisearchApiError as _:
             self.client.create_index(self.index)
         # make a variable to easily reference the index
         self.idxref = self.client.index(self.index)
-
+        time.sleep(0.05)
         # update filterable attributes if needed
+        self.idxref.update_distinct_attribute('partnum')
         self.update_filterables(filterable_attrs)
 
     def add_document(self, document: dict) -> TaskInfo:
@@ -65,11 +70,10 @@ class JukeboxSearch:
         
         :param filterables: List of all filterable attributes"""
 
-        existing_filterables = self.idxref.get_filterable_attributes()
-        if len(set(existing_filterables).difference(set(filterables))) > 0:
-            taskref = self.idxref.update_filterable_attributes(filterables)
-
-            self.client.wait_for_task(taskref.index_uid)
+        #existing_filterables = self.idxref.get_filterable_attributes()
+        #if len(set(existing_filterables).difference(set(filterables))) > 0:
+        taskref = self.idxref.update_filterable_attributes(filterables)
+        #self.client.wait_for_task(taskref.index_uid)
 
     def search(self, query: str, filters: str = None):
         """Execute a search query on the Meilisearch index.
@@ -90,7 +94,7 @@ class JukeboxSearch:
         :returns: A dict containing the results; If no results found, an empty dict."""
         q = self.search("", filter)
         if q["estimatedTotalHits"] != 0:
-            return ["hits"][0]
+            return q["hits"][0]
         else:
             return dict()
 
@@ -114,4 +118,4 @@ class JukeboxSearch:
 
 # entrypoint
 if __name__ == "__main__":
-    jbs = JukeboxSearch()
+    jbs = JukeboxSearch()

setup-alpine-vm.sh

@@ -0,0 +1,20 @@
+#!/bin/sh
+
+# This script must run as root!
+
+echo "https://dl-cdn.alpinelinux.org/alpine/latest-stable/main
+https://dl-cdn.alpinelinux.org/alpine/latest-stable/community" > /etc/apk/repositories
+
+apk upgrade
+apk add git docker docker-cli-compose
+
+rc-update add docker
+service docker start
+
+git clone https://git.myitr.org/Jukebox/jukebox-software
+cd jukebox-software
+git submodule init
+git submodule update
+
+docker compose build
+docker compose up -d

tempCodeRunnerFile.py

@@ -0,0 +1,2 @@
+
+    drop_off_tray(robot, 0)

test.py

@@ -1,4 +1,46 @@
-print("\u001b[37m")
+from pyModbusTCP.client import ModbusClient
+
+
+
+def get_sensors():
+    mbconn = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=True)
+    """
+    port 1: 256
+    port 2: 272
+    port 3: 288
+    port 4: 304
+    port 5: 320
+    port 6: 336
+    port 7: 352
+    port 8: 368
+    """
+    out = list()
+    for reg in [352, 288, 304, 368]:
+        val = mbconn.read_holding_registers(reg)[0] # read only one register
+        print(val)
+        if val == 1:
+            out.append(True)
+        else:
+            out.append(False)
+
+    return out
+
+def get_open_spot(sensordata):
+    for x in range(len(sensordata)):
+        sens = sensordata[x]
+        if not sens:
+            return x
+        
+    # if we get here, every spot is full
+        
+    return False
+
+
+testmb = get_sensors()
+print(testmb)
+print("Spot open", get_open_spot(testmb))
+
+exit()
 
 class Ring:
     def __init__(self) -> None:

ur5_control.py

@@ -5,54 +5,109 @@ import math
 import numpy as np
 import time
 import os
-import logging
-from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
+#import logging
+import yaml
 import sys
 from util import fprint
+from pyModbusTCP.client import ModbusClient
+
+import subprocess
+from util import win32
 
 
 
+class Rob():
+    robot = None
+    #offset_x, offset_y, offset_z = (0, 0, 0.14)     # Tool offset
+    #
+    def __init__(self, config):
+        self.config = config
+        armc = config["arm"]
+        self.ip = armc["ip"]
+        tool = armc["tool"]
+        limbs = armc["limbs"]
+        self.offset_x, self.offset_y, self.offset_z = (tool["offset_x"], tool["offset_y"], tool["offset_z"])
+        self.limb_base = limbs["limb_base"]
+        self.limb1 = limbs["limb1"]
+        self.limb2 = limbs["limb2"]
+        self.limb3 = limbs["limb3"]
+        self.limb_wrist = limbs["limb_wrist"]
+        #self.init_arm()
 
-rob = None
-offset_x, offset_y, offset_z = (0, 0, 0.14)     # Tool offset
-limb_base, limb1, limb2, limb3, limb_wrist = (0.105, .425, .39225, .1, .0997)    # Limb lengths
+def ping(host):
+        #Returns True if host (str) responds to a ping request.
 
-def init(ip):
-    global rob
+        # Option for the number of packets as a function of
+        if win32:
+            param1 = '-n'
+            param2 = '-w'
+            param3 = '250'
+        else:
+            param1 = '-c'
+            param2 = '-W'
+            param3 = '0.25'
+
+        # Building the command. Ex: "ping -c 1 google.com"
+        command = ['ping', param1, '1', param2, param3, host]
+
+        return subprocess.call(command, stdout=subprocess.DEVNULL, stderr=subprocess.STDOUT) == 0
+
+def powerup_arm(robot):
     #sys.stdout = Logger()
     fprint("Starting UR5 power up...")
-    
-    # power up robot here
+    # power up robot here via PCB
 
+    #
     # wait for power up (this function runs async)
 
-
+    while not ping(robot.ip):
+        time.sleep(0.5)
     # trigger auto-initialize
-
+    fprint("Arm online. Waiting for calibration.")
     # wait for auto-initialize
+def connect(robot):
+    if robot.robot is None:
+        newrobot = Rob(robot.config)
+        robot = newrobot
+        ip = robot.ip
+        fprint("Connecting to arm at " + ip)
+        trying = True
+        while trying:
+            try:
+                robot.robot = urx.Robot(ip)
+                trying = False
+            except:
+                time.sleep(1)
+        # Sets robot arm endpoint offset (x,y,z,rx,ry,rz)
+        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))
+    return robot
 
+def init_arm(robot):
+    robot = connect(robot)
+    
     # init urx
-    fprint("Connecting to arm at " + ip)
-    trying = True
-    while trying:
-        try:
-            rob = urx.Robot(ip)
-            trying = False
-        except:
-            time.sleep(1)
-    robotiqgrip = Robotiq_Two_Finger_Gripper(rob)
-
-    # Sets robot arm endpoint offset (x,y,z,rx,ry,rz)
-    rob.set_tcp((0, 0, 0.15, 0, 0, 0))
-
-    # Set weight
-    rob.set_payload(2, (0, 0, 0.1))
+    
     #rob.set_payload(2, (0, 0, 0.1))
     time.sleep(0.2)
     fprint("UR5 ready.")
+    #return robot.robot
 
-def set_pos_abs(x, y, z, xb, yb, zb, threshold=None):
-    global rob
+    # setup - in case of fail. open gripper, move up, then go home.
+    rob = robot.robot
+    open_gripper()
+    curr_pos = rob.getl()
+    new_pos = curr_pos
+    new_pos[2] += 0.025
+    rob.movel(new_pos, vel=0.05, acc=1)
+
+    move_to_home(robot, speed=0.5)
+
+    return True
+
+def set_pos_abs(robot, x, y, z, xb, yb, zb, threshold=None):
+    rob = robot.robot
     new_orientation = m3d.Transform()
     new_orientation.orient.rotate_xb(xb)  # Replace rx with the desired rotation around X-axis
     new_orientation.orient.rotate_yb(yb)  # Replace ry with the desired rotation around Y-axis
@@ -68,8 +123,8 @@ def set_pos_abs(x, y, z, xb, yb, zb, threshold=None):
     #rob.speedj(0.2, 0.5, 99999)
     rob.set_pose(new_trans, acc=2, vel=2, command="movej", threshold=threshold)  # apply the new pose
 
-def set_pos_rel_rot_abs(x, y, z, xb, yb, zb):
-    global rob
+def set_pos_rel_rot_abs(robot, x, y, z, xb, yb, zb):
+    rob = robot.robot
     new_orientation = m3d.Transform()
     new_orientation.orient.rotate_xb(xb)  # Replace rx with the desired rotation around X-axis
     new_orientation.orient.rotate_yb(yb)  # Replace ry with the desired rotation around Y-axis
@@ -86,8 +141,8 @@ def set_pos_rel_rot_abs(x, y, z, xb, yb, zb):
     #rob.speedj(0.2, 0.5, 99999)
     rob.set_pose(new_trans, acc=0.1, vel=0.4, command="movej")  # apply the new pose
 
-def set_pos_abs_rot_rel(x, y, z, xb, yb, zb):
-    global rob
+def set_pos_abs_rot_rel(robot, x, y, z, xb, yb, zb):
+    rob = robot.robot
     new_orientation = m3d.Transform()
     new_orientation.orient.rotate_xb(xb)  # Replace rx with the desired rotation around X-axis
     new_orientation.orient.rotate_yb(yb)  # Replace ry with the desired rotation around Y-axis
@@ -132,9 +187,8 @@ def polar_to_cartesian(r, theta):
     y = r * np.sin(theta)
     return x, y
 
-
-def move_to_polar(start_pos, end_pos):
-    global rob
+def move_to_polar(robot, start_pos, end_pos):
+    rob = robot.robot
 
     # Convert to polar coordinates
     start_r, start_theta = cartesian_to_polar(start_pos[0], start_pos[1])
@@ -191,24 +245,34 @@ def move_to_polar(start_pos, end_pos):
 
     return rx_intermediate
 
-def degtorad(angle):
-        return angle/180.0 * math.pi
-def radtodeg(angle):
-        return angle*180.0 / math.pi
 
-def move_to_home():
-    global rob
+def move_to_home(robot, keep_flip=False, speed=2):
+    rob = robot.robot
+
+    if is_flipped(robot) and not keep_flip:
+        flip(robot)
+
+    # Move robot to home position
+    rob.movej(offset_gripper_angle(robot, *(-0.18, -0.108, 0.35), flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position
+
+    return True
+
+def move_to_packup(robot, speed=0.25):
+    rob = robot.robot
+    
+    # known good starting point to reach store position
+    goto_holder_index(robot, 12, 0.3, flip=False, use_closest_path=False)
 
     # Home position in degrees
-    home_pos = [0.10421807948612624, 
-                -2.206111555015423, 
-                1.710679229503537, 
-                -1.075834511928354, 
-                -1.569301366430687, 
-                1.675098295930943]
-
+    store_pos = [-1.5708, 
+                -1.3, 
+                2.362, 
+                0.7056, 
+                -1.425, 
+                1.5708]
     # Move robot
-    rob.movej(home_pos, acc=2, vel=2)
+    rob.movej(store_pos, acc=0.1, vel=speed)
+    return True
 
 def normalize_degree(theta):
     # Normalizes degree theta from -1.5pi to 1.5pi
@@ -222,15 +286,14 @@ def normalize_degree(theta):
     # Return angle
     return normalized_theta
 
-
-
-def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess = (math.pi/2, math.pi/2, 0), l3offset=0):
+def get_joints_from_xyz_rel(robot, x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess = (math.pi/2, math.pi/2, 0), l3offset=0):
     # Get limbs and offsets
-    
     #l3=0.15
-    l_bs, l1, l2, l3, l_wt = (limb_base, limb1, limb2, limb3, limb_wrist)    # Limb lengths
+    l_bs, l1, l2, l3, l_wt = (robot.limb_base, robot.limb1, robot.limb2, robot.limb3, robot.limb_wrist)    # Limb lengths
     l3 += l3offset # add wrist offset, used for gripper angle calculations
-    
+    offset_x = robot.offset_x
+    offset_y = robot.offset_y
+    offset_z = robot.offset_z
     # Calculate base angle and r relative to shoulder joint
     def calculate_theta(x, y, a):
         # Calculate if we need the + or - in our equations
@@ -273,200 +336,395 @@ def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess =
     # Return result
     return base, shoulder, elbow, wrist1, ry, rz
 
-def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0):
-    joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz, l3offset=l3offset)
+def get_joints_from_xyz_abs(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0, use_closest_path=True):
+    rob = robot.robot
+    joints = get_joints_from_xyz_rel(robot, x, y, z, rx, ry, rz, l3offset=l3offset)
+
+    # Return current positions if coordinates don't make sense
+    if z<0:
+        return rob.getj()
 
     # Joint offsets
     # Base, Shoulder, Elbow, Wrist
     inverse = [1, -1, 1, 1, 1, 1]
     offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0]
-    if radtodeg(joints[1]) > 137:
+
+    if math.degrees(joints[1]) > 137:
         print("CRASH! Shoulder at", joints[1] * 180/math.pi)
     #else:
         #print("Shoulder at", joints[1] * 180/math.pi)
-    # Return adjusted joint positions
-    return [o+j*i for j, o, i in zip(joints, offsets, inverse)]
 
-# gripper angle: from vertical
-# gripper length: from joint to start of grip
-# to flip, you can use flip=True or make gripper angle negative
-def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.20+0.018, flip=False):
+    # Get adjusted joint positions
+    adjusted_joints =  [o+j*i for j, o, i in zip(joints, offsets, inverse)]
+
+    curr_joints = rob.getj()
+    def get_complimentary_angle(joint_angle):
+        if joint_angle<0:
+            new_angle = joint_angle + 2*math.pi
+        else:
+            new_angle = joint_angle - 2*math.pi
+        
+        if abs(new_angle) > math.radians(350):
+            return joint_angle
+        else:
+            return new_angle
+        
+    # Use closest path (potentially going beyond 180 degrees)
+    if use_closest_path:
+        if abs(get_complimentary_angle(adjusted_joints[0])-curr_joints[0]) < abs(adjusted_joints[0]-curr_joints[0]):
+            adjusted_joints[0] = get_complimentary_angle(adjusted_joints[0])
+
+    # final_joint_positions = []
+    # for curr_joint, adjusted_joint in zip(curr_joints, adjusted_joints):
+    #     if abs(curr_joint - adjusted_joint) < abs(curr_joint - get_complimentary_angle(adjusted_joint)):
+    #         final_joint_positions.append(adjusted_joint)
+    #     else:
+    #         final_joint_positions.append(get_complimentary_angle(adjusted_joint))
+
+    # return final_joint_positions
+        
+    return adjusted_joints
+
+
+def move_arc(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
+    rob = robot.robot
+    
+    start_joints = rob.getj()
+    end_joint = get_joints_from_xyz_abs(robot, x, y, z, rx, ry, rz)
+
+    n_points = 50
+    intermediate_joints = []   
+    for i in range(0, 6):
+        intermediate_joints.append(np.linspace(start_joints[i], end_joint[i], n_points))
+
+    joints = [joint_position for joint_position in zip(*intermediate_joints)]
+
+    rob.movejs(joints, acc=2, vel=2, radius=0.1)
+
+def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.018, flip=False, use_closest_path=True, rzoffset=0):
+    # gripper angle: from vertical
+    # gripper length: from joint to start of grip
+    # to flip, you can use flip=True or make gripper angle negative
+    limb3 = robot.limb3
+    # Determine tool rotation depending on gripper angle
     if gripperangle < 0:
         rz = - math.pi / 2
     else:
         rz = math.pi / 2
 
     if flip:
-        gripperangle = -degtorad(gripperangle)
+        gripperangle = -math.radians(gripperangle)
         grippery = gripperlength - math.cos(gripperangle) * gripperlength
         grippery += math.sin(gripperangle) * limb3
         gripperx = math.sin(gripperangle) * gripperlength + limb3 * 2
         gripperx -= (1-math.cos(gripperangle)) * limb3
         rz = math.pi / 2
         # flip the whole wrist
-        return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle + degtorad(180), l3offset=-gripperx, ry=math.pi/2, rz=rz)
+        return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=-3*math.pi/2, rz=rz + rzoffset, use_closest_path=use_closest_path)
+
     else:
-        gripperangle = degtorad(gripperangle)
+        gripperangle = math.radians(gripperangle)
         grippery = gripperlength - math.cos(gripperangle) * gripperlength
         grippery -= math.sin(gripperangle) * limb3
         gripperx = math.sin(gripperangle) * gripperlength
         gripperx += (1-math.cos(gripperangle)) * limb3
 
-        return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz)
+        return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz, use_closest_path=use_closest_path)
+    
+def goto_holder_index(robot, idx, z=0.05, gripperangle=30, flip=False, use_closest_path=True, verbose=False):
+    joint = robot.config["position_map"][idx]
+    
+    if verbose:
+        print("Going to cable holder index", joint["index"], "at position", joint["pos"])   
+    
+    safe_move(robot, joint["pos"][0]/1000, joint["pos"][1]/1000, z, use_closest_path=use_closest_path)
+    #angles = offset_gripper_angle(joint["pos"][1]/1000, joint["pos"][0]/1000, z, gripperangle=gripperangle, flip=flip)
+    #rob.movej(angles, acc=2, vel=2)
+    #return angles
+    #angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )
+    return True
+
+def is_flipped(robot):
+    rob = robot.robot
+    wrist1 = rob.getj()[3]
+    
+    if wrist1 > 0:
+        return True
+    else:
+        return False
+
+def flip(robot):
+    rob = robot.robot
+
+    # A list of safe positions to flip
+    safe_positions = [(-0.18, -0.108, 0.35),
+                      (0.18, -0.108, 0.35)]
     
 
-def goto_holder_index(idx, z=0.05, gripperangle=35, flip=False):
-    joint = config["position_map"][idx]
-    print("Going to cable holder index", joint["index"], "at position", joint["pos"])   
-    angles = offset_gripper_angle(joint["pos"][1]/1000, joint["pos"][0]/1000, z, gripperangle=gripperangle, flip=flip)
-    #rob.movej(angles, acc=2, vel=2)
-    return angles
-    #angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )
+    # Find the closest safe position
+    curr_pos = rob.getl()[:3]
+    def dist_from_robot(pos):
+        x, y, z = pos
+        rx, ry, rz = curr_pos
+        return math.sqrt((rx-x)**2+(ry-y)**2+(rz-z)**2)
+
+    pos_dist_pairs = zip(safe_positions, [dist_from_robot(pos) for pos in safe_positions])
+    safe_pos = min(pos_dist_pairs, key=lambda x:x[1])[0]
+
+    # Flip at safe position
+    rob.movej(offset_gripper_angle(robot, *safe_pos, flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position
+    rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped(robot))), vel=2, acc=2) # Flip gripper
+    # print('flip?: ', is_flipped(robot))
+    return True
+
+def safe_move(robot, x, y, z, use_closest_path=True):
+    rob = robot.robot
+    flip_radius = 0.22 # Min radius on which to flip
+    r = math.sqrt(x**2 + y**2) # Get position radius
+
+    # Flip gripper if needed
+    if (r <= flip_radius and is_flipped(robot)) or (r > flip_radius and not is_flipped(robot)):
+        flip(robot)
+
+    rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot), use_closest_path=use_closest_path), vel=4, acc=3)
+    return True
+
+def holder_routine(robot, holder_index, pick_up, verbose=False):
+    robot = connect(robot)
+    rob = robot.robot
+    
+    # Don't attempt to place a tube in the camera slot
+    if holder_index == 49:
+        return
+
+    if verbose:
+        fprint('Pickup routine for index' + str(holder_index))
+
+    # Go to the correct holder
+    if pick_up:
+        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)
+    
+    if pick_up:
+        open_gripper()
+
+    # Move down
+    curr_pos = rob.getl()
+    new_pos = curr_pos
+    new_pos[2] = 0.005
+    rob.movel(new_pos, vel=0.1, acc=1)
+    
+    # Pick up or drop off
+    if pick_up:
+        close_gripper()
+    else:
+        open_gripper()
+
+    # Move up
+    new_pos[2] = 0.2
+    rob.movel(new_pos, vel=2, acc=1)
+    was_flipped = is_flipped(robot)
+
+    # 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 tray_routine(robot, slot=0, pick_up=True):
+    robot = connect(robot)
+    rob = robot.robot
+
+    # Default to 0 if invalid value
+    if slot not in [0, 1, 2, 3]:
+        slot = 0
+
+    slot_prepositions =  [(-9.93, -112.67, 144.02, -116.69, -54.13, -10.29),
+                         (-12.35, -124.95, 148.61, -107.27, -54.36, -13.26),
+                         (-16.45, -96.97, 137.85, 58.39, -305.08, 161.75),
+                         (-16.66, -97.28, 138.16, 58.54, -305.05, 161.50)] 
+
+    # Initial position depending on slot and robot orientation
+    if slot in [0, 1]:
+        if is_flipped(robot):
+            flip(robot)
+        else:
+            move_to_home(robot, keep_flip=True)
+    else:
+        goto_holder_index(robot, 25, z=0.3)
+
+    # Align robot to the slot
+    if slot in [2,3]:
+        angles = [(-2.77, -99.64, 131.02, 67.67, 70.04-360, 153.03),
+                  slot_prepositions[slot]]
+    else:
+        angles = [(-58, -114.45, 100.52, -45.24, -96.95, 120),
+                  (-39.98, -124.92, 132.28, -61.56, -55.60, -50.77),
+                  slot_prepositions[slot]]
+        
+    angles = [[x*math.pi/180 for x in move] for move in angles]
+    rob.movejs(angles,vel=2,acc=1) 
+
+   # Positions for each slot
+    slot_distance = .052
+    slot_height = -.015-.0095
+    first_slot = -0.3084+0.02
+    slot_position = [
+        [first_slot, -0.3426, slot_height, 1.5899, 1.5526, -0.9411],
+        [first_slot+slot_distance, -0.3426, slot_height, 1.5899, 1.5526, -0.9411],
+        [first_slot+2*slot_distance, -0.3426, slot_height, 1.5899, 1.5526, -0.9411],
+        [first_slot+3*slot_distance, -0.3426, slot_height, 1.5899, 1.5526, -0.9411],
+    ]
+    rob.movel(slot_position[slot],vel=0.2, acc=1)
+
+    # Place/Grab the tube
+    if pick_up:
+        close_gripper()
+    else:
+        open_gripper()
+
+
+    # Move back
+    tilt = 0.3
+    curr_pos = rob.getl()
+    new_pos = curr_pos
+    if slot==3:
+        new_pos[0] -= 0.05  #x
+    new_pos[1] += 0.15      #y
+    new_pos[2] = 0.09       #z
+    new_pos[3] += tilt
+    new_pos[4] += tilt
+    new_pos[5] += tilt
+    rob.movel(new_pos, vel=0.2, acc=1)
+    
+
+    # Go home to safe position
+    move_to_home(robot, speed=1, keep_flip=True)  
+def pick_up_tray(robot, slot=0):
+    tray_routine(robot, slot, True)
+def drop_off_tray(robot, slot=0):
+    tray_routine(robot, slot, False)
+
+
+def return_routine(robot, slot, holder_index=None, verbose=False):
+    robot = connect(robot)
+    rob = robot.robot
+
+    
+    open_gripper()
+    was_flipped = is_flipped(robot)
+    if slot is None:
+        rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3)
+        rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=4, acc=3)
+        close_gripper()
+    else:
+        xoffset = 0.051 * slot
+        rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3)
+        rob.movej(get_joints_from_xyz_abs(robot, -0.35+xoffset, -0.15, 0.0, math.pi/2, 0.1), vel=4, acc=3)
+        close_gripper()
+
+
+    if holder_index is not None:
+        goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
+        curr_pos = rob.getl()
+        new_pos = curr_pos
+        new_pos[2] = 0.015
+        rob.movel(new_pos, vel=0.1, acc=1)
+        open_gripper()
+        new_pos[2] = 0.1
+        rob.movel(new_pos, vel=2, acc=1)
+        return True
+    else:
+        # go to camera
+        rob.movej(offset_gripper_angle(robot, 0.35, -0.35, 0.3, flip=was_flipped, use_closest_path=False), vel=2, acc=2)
+        return True
+    
+
+def goto_camera(robot):
+    robot = connect(robot)
+    goto_holder_index(robot, 49, 0.2)
+
+def tray_to_camera(robot, slot):
+    pick_up_tray(robot, slot)
+    goto_camera(robot)
+def holder_to_tray(robot, holder_index, slot):
+    pick_up_holder(robot, holder_index)
+    drop_off_tray(robot, slot)
+
+def holder_to_camera(robot, 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):
+    robot = connect(robot)
+    rob = robot.robot
+
+    drop_off_holder(robot, holder_index)
+    
+
+def open_gripper():
+    fprint("Opening gripper")
+    c = ModbusClient(host="192.168.1.21", port=502, auto_open=True, auto_close=False)
+    
+    c.write_single_register(112, 0b0)
+    c.write_single_register(435, 0b10000000)
+
+    c.write_single_register(112, 0b0)
+    c.write_single_register(435, 0b10000000)
+    time.sleep(0.5)
+    c.close()
+    
+    #c.close()
+
+def close_gripper():
+    fprint("Closing gripper")
+    c = ModbusClient(host="192.168.1.21", port=502, auto_open=True, auto_close=False)
+    c.write_single_register(435, 0b00000000)
+    c.write_single_register(112, 0b1)
+
+    c.write_single_register(435, 0b00000000)
+    c.write_single_register(112, 0b1)
+    time.sleep(0.5)
+    c.close()
+    #
 
 if __name__ == "__main__":
     
-    #rob.movej((0, 0, 0, 0, 0, 0), 0.1, 0.2)
-    #rob.movel((x, y, z, rx, ry, rz), a, v)
-    init("192.168.1.145")
-    print("Current tool pose is: ",  rob.getl())
-    move_to_home()
-
-    home_pose = [-0.4999999077032916, 
-               -0.2000072960336574, 
-               0.40002172976662786, 
-               0, 
-               -3.14152741295329, 
-               math.radians(62)]
-
-    # time.sleep(.5)
-
-    p1 = [0,
-
-        0.6,
-        .4,
-        0.2226,
-        3.1126,
-        0.0510]
-    
-    p2 = [0.171,
-        -0.115,
-        0.2,
-        0.2226,
-        3.1126,
-        0.0510]
-
-    curr_pos = rob.getl()
-    # up/down, 
-    # tool rotation
-    # tool angle (shouldn't need)
-    # rob.set_pos(p1[0:3], acc=0.5, vel=0.5)
-
-    # set_pos_abs(*home_pose)
-
-
-    
-    # angles = get_joints_from_xyz_abs(-0.2, 0, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(-0.2, -0.2, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.6, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.5, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.4, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.3, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.2, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.13, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    config = None
-    joints = []
-    for i in np.linspace(-0.2, -0.7, 50):
-         joints.append(get_joints_from_xyz_abs(i, 0, 0))
-    #rob.movejs(joints, acc=2, vel=2)
-    import yaml
+    #init("192.168.1.145")
     with open('config.yml', 'r') as fileread:
         #global config
         config = yaml.safe_load(fileread)
-
-    #rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2)
-    #joints = []
-    
-    #for i in np.linspace(0, 340, 340):
-    #    joints.append(goto_holder_index(24, 0.5, i))
-    #rob.movejs(joints, acc=1, vel=3)
-    angle = 30
-    rob.movej(goto_holder_index(26, 0.1, angle), acc=2, vel=2)
-    time.sleep(1)
-    rob.movej(goto_holder_index(25, 0.1, angle), acc=2, vel=2)
-    time.sleep(1)
-    rob.movej(goto_holder_index(24, 0.1, angle, flip=True), acc=2, vel=2)
-    #rob.movej(goto_holder_index(32, 0.2, angle), acc=2, vel=2)
-    #rob.movej(goto_holder_index(38, 0.2, angle), acc=2, vel=2)
-    #rob.movej(goto_holder_index(25, 0.1, angle, flip=True), acc=2, vel=2)
-
-    #rob.movej(goto_holder_index(25, 0.2, angle, flip=True), acc=2, vel=2)
-    #rob.movej(goto_holder_index(24, 0.1, angle, flip=True), acc=2, vel=2)
-    #time.sleep(1)
-    #rob.movej(goto_holder_index(25, 0.1, angle, flip=True), acc=2, vel=2)
-    #rob.movej(goto_holder_index(49, 0.1, angle), acc=2, vel=2)
-    #rob.movej(goto_holder_index(49, 0.1, angle, flip=True), acc=2, vel=2)
-    # rob.movej(goto_holder_index(50, 0.1, angle, flip=True), acc=2, vel=2)
-    # rob.movej(goto_holder_index(51, 0.1, angle, flip=True), acc=2, vel=2)
-    # rob.movej(goto_holder_index(52, 0.1, angle, flip=True), acc=2, vel=2)
-    # rob.movej(goto_holder_index(53, 0.1, angle, flip=True), acc=2, vel=2)
-    #time.sleep(2)
-    #rob.movej(goto_holder_index(24, 0.15, 35, flip=True), acc=2, vel=2)
-    #time.sleep(10)
-    # time.sleep(4)
-    # goto_holder_index(26, 0.1, 20)
-    # time.sleep(4)
-    # goto_holder_index(26, 0.1, 30)
-    # time.sleep(4)
-    # goto_holder_index(26, 0.1, 40)
-    # for joint in config["position_map"]:
-    #joint = config["position_map"][26]
-    #print("Going to cable holder index", joint["index"], "at position", joint["pos"])   
-    #angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )# rx=math.pi / 5)
-    #joints.append(angles)
-
-    #rob.movej(angles, acc=2, vel=2)
-    #time.sleep(10)
-    #rob.movejs(joints, acc=2, vel=2)
-    # joints = []
-    # for i in np.linspace(-0.3, -0.7, 50):
-    #     joints.append(get_joints_from_xyz_abs(i, 0, 0))
-    # rob.movejs(joints, acc=2, vel=2)
-
-    # time.sleep(5)
-
-
-    # angles = get_joints_from_xyz_abs(0, -0.6, 0)
-    # rob.movej(angles, acc=2, vel=2)
     
 
 
-    # set_pos_abs(*p1)
-    # move = move_to_polar(p1, p2)
+    robot = Rob(config) # robot of type Rob is the custom class above
+    #powerup_arm(robot)
+    robot = connect(robot)
+    init_arm(robot)
+    
+    rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously
 
 
-    # for p in move:
-    #     print(math.degrees(p))
-    # print("Safe? :", is_safe_move(p1, p2))
+    #move_to_packup(robot)
+    
 
+    # for i in range(3):
+    #     pick_up_holder(robot, i)
 
-    # #set_pos_rel_rot_abs(0, 0, -0.2, math.pi, 0, -math.pi)
-    # set_pos_abs(0.3, -0.2, 0.5, math.pi, 0, -math.pi)
-    # set_pos_abs(0, 0.2, 0.6, math.pi, 0, -math.pi)
-    # set_pos_abs(-0.5, -0.2, 0.4, math.pi, 0, -math.pi)
-    # #set_pos_rel_rot_abs(0, 0, 0, math.pi, 0, -math.pi)
+    #     print('Drop off', i+1)
+    #     drop_off_holder(robot, i+1)
 
-    # print("Current tool pose is: ",  rob.getl())
-    # print("getj(): ",  rob.getj())
+    holder_to_camera(robot, 0)
+    camera_to_holder(robot, 0)
 
-    # move_to_home()
+    print("Current tool pose is: ",  rob.getl())
+    curr_pos = rob.getl()
+    config = None
+  
 
     rob.stop()
     os.kill(os.getpid(), 9) # dirty kill of self

websocket_test.html

@@ -81,7 +81,7 @@
                 socket.send(message);
                 console.log('Message sent', message);
             }
-            setInterval(ping, 1500);
+            //setInterval(ping, 1500);
 
             // setInterval(() => {
             //     updateServiceStatus('serviceA', 'down');