Restart VM to clear network leftovers

.gitignore

@@ -21,3 +21,5 @@ map*.png
 build
 # Generated label images
 labels
+temp
+pick_count.txt

cables.txt

@@ -0,0 +1,26 @@
+1172C
+86104CY
+FIT-221-1_4
+10GXW13
+10GXW53
+29501F
+29512
+3092A
+3105A
+3106A
+6300FE
+6300UE
+7922A
+7958A
+8760
+9841
+FI4X012W0
+FISX012W0
+IOP6U
+RA500P
+SPE101
+SPE102
+TF-1LF-006-RS5
+TF-SD9-006-RI5
+TT-SLG-024-HTN
+3050

client.bat

@@ -0,0 +1,29 @@
+
+@echo off
+
+:: BatchGotAdmin
+:-------------------------------------
+REM  --> Check for permissions
+>nul 2>&1 "%SYSTEMROOT%\system32\cacls.exe" "%SYSTEMROOT%\system32\config\system"
+
+REM --> If error flag set, we do not have admin.
+if '%errorlevel%' NEQ '0' (
+    echo Requesting administrative privileges...
+    goto UACPrompt
+) else ( goto gotAdmin )
+
+:UACPrompt
+    echo Set UAC = CreateObject^("Shell.Application"^) > "%temp%\getadmin.vbs"
+    set params = %*:"=""
+    echo UAC.ShellExecute "cmd.exe", "/c %~s0 %params%", "", "runas", 1 >> "%temp%\getadmin.vbs"
+
+    "%temp%\getadmin.vbs"
+    del "%temp%\getadmin.vbs"
+    exit /B
+
+:gotAdmin
+    pushd "%CD%"
+    CD /D "%~dp0"
+    python run.py
+:--------------------------------------
+

config.yml

@@ -3,7 +3,11 @@ core:
   serverip: 172.26.178.114
   clientip: 172.26.176.1
   server: Hyper-Vd
-  loopspeed: 100 # fps
+  loopspeed: 60 # fps
+
+mqtt:
+  enabled: True
+  server: 172.31.108.4
 
 arm:
   ip: 192.168.1.145
@@ -24,10 +28,10 @@ cables:
 
 cameras:
   banner:
-    ip: 192.168.1.125
+    ip: 192.168.1.199
-    port: 32200
+    port: 80
 
-animation_time: 100
+animation_time: 60
 
 led: 
   fps: 100
@@ -81,15 +85,15 @@ led:
     - universe: 10
       ip: 192.168.1.209
       mode: solid
-      color: [0, 50, 150]
+      color: [0, 50, 100]
     - universe: 11
       ip: 192.168.1.210
       mode: solid
-      color: [0, 50, 150]
+      color: [0, 50, 100]
     - universe: 12
       ip: 192.168.1.211
       mode: solid
-      color: [0, 50, 150]
+      color: [0, 50, 100]
     # - universe: 0
     #   ip: 192.168.1.209
     #   ledstart: 1296
@@ -264,7 +268,7 @@ led:
       size: 24
       diameter: 63.5
       angle: 180
-      pos: [197.973, 38.1]
+      pos: [201.973, 34.1]
     - type: circle
       start: 624
       size: 24
@@ -470,7 +474,7 @@ position_map:
   - index: 7
     pos: [-38.1, 197.973]
   - index: 8
-    pos: [38.1, 197.973]
+    pos: [34.1, 201.973]
   - index: 9
     pos: [114.3, 197.973]
   - index: 10

fileserver.py

@@ -2,16 +2,22 @@ import http.server
 import socketserver
 import os
 
+class CORSHTTPRequestHandler(http.server.SimpleHTTPRequestHandler):
+    def end_headers(self):
+        self.send_header('Access-Control-Allow-Origin', '*')
+        self.send_header('Access-Control-Allow-Methods', 'GET, POST, OPTIONS')
+        self.send_header('Access-Control-Allow-Headers', 'X-Requested-With, Content-Type')
+        http.server.SimpleHTTPRequestHandler.end_headers(self)
+
 def run_server(port, directory):
     """
-    Run a simple HTTP server serving files from the specified directory.
+    Run a simple HTTP server serving files from the specified directory with CORS enabled.
     """
     # Change the working directory to the specified directory
     os.makedirs(directory, exist_ok=True)
     os.chdir(directory)
 
-    # Create the HTTP server
+    # Create the HTTP server using the CORS-enabled handler
-    handler = http.server.SimpleHTTPRequestHandler
+    with socketserver.TCPServer(("", port), CORSHTTPRequestHandler) as httpd:
-    with socketserver.TCPServer(("", port), handler) as httpd:
+        print(f"Serving files at port {port} with CORS enabled")
-        print(f"Serving cable images & files at port {port}")
         httpd.serve_forever()

label_document.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+import fitz  # PyMuPDF
+from reportlab.pdfgen import canvas
+from reportlab.lib.pagesizes import letter
+import os
+
+
+def generate_pdf(path="labels"):
+    # Open the existing PDF
+    image_folder_path = path
+    doc = fitz.open('label_template.pdf')
+    page = doc[0]  # Assuming you want to read from the first page
+
+    placeholders = []
+    for shape in page.get_drawings():
+        if shape['type'] == 's':  # Checking for rectangle types
+            placeholders.append({
+                'x': shape['rect'].x0,
+                'y': shape['rect'].y0,
+                'width': shape['rect'].width,
+                'height': shape['rect'].height
+            })
+
+    # List all PNG images in the folder
+    image_files = [f for f in os.listdir(image_folder_path) if f.endswith('.png')]
+    image_paths = [os.path.join(image_folder_path, file) for file in image_files]
+
+    # Create a new PDF with ReportLab
+    c = canvas.Canvas(path + "/print.pdf", pagesize=letter)
+    current_placeholder = 0  # Track the current placeholder index
+
+    for image_path in image_paths:
+        if current_placeholder >= len(placeholders):  # Check if a new page is needed
+            c.showPage()
+            current_placeholder = 0  # Reset placeholder index for new page
+
+        # Get current placeholder
+        placeholder = placeholders[current_placeholder]
+
+        # Place image at the placeholder position
+        c.drawImage(image_path, placeholder['x'], page.rect.height - placeholder['y'] - placeholder['height'], width=placeholder['width'], height=placeholder['height'])
+        current_placeholder += 1
+
+    # Save the final PDF
+    c.save()
+
+    # Close the original PDF
+    doc.close()
+
+if __name__ == "__main__":
+    generate_pdf("labels")

label_generator.py

@@ -9,6 +9,7 @@ import os
 import signal
 from PIL import Image
 from label_image import generate_code
+from label_document import generate_pdf
 
 
 def input_cable():
@@ -22,7 +23,7 @@ def input_cable():
     print("Input part number:", inputnum)
     print("Searching databases for cables...")
     # Search both AW and BL sites
-    status, output = get_multi(["BL"+inputnum, "AW"+inputnum], delay=0.1, dir="temp/" + str(uuid.uuid4()) + "/", cache=False)
+    status, output = get_multi(["BL"+inputnum, "AW"+inputnum], delay=0.1, dir="temp/" + str(uuid.uuid4()) + "/", webport=":9000", cache=False)
     print("")
     if len(output) > 1:
         for i in output:
@@ -70,6 +71,8 @@ def gen_label(partnum, path="labels"):
     img = generate_code(partnum)
     os.makedirs(path, exist_ok=True)
     img.save(path + "/" + partnum + ".png")
+    generate_pdf(path)
+
 
 def delete_folder(path):
     # Check if the path is a directory

label_image.py

@@ -269,10 +269,10 @@ def qr_image(data, width=600):
     png_image_io = "belden-logo-superhires.png"
     png_image_pillow = Image.open(png_image_io)
     png_width, png_height = png_image_pillow.size
-    png_image_pillow = png_image_pillow.resize((int(width*5.2), int(width*5.2/png_width*png_height)))
+    png_image_pillow = png_image_pillow.resize((int(width*4), int(width*4/png_width*png_height)))
     png_width, png_height = png_image_pillow.size
     # paste belden logo first because it has a big border that would cover stuff up
-    img.paste(png_image_pillow, (int(width*5-png_width/2), int(width*4.25 - png_height/2)))
+    img.paste(png_image_pillow, (int(width*5-png_width/2), int(width*3.25 - png_height/2)))
 
     # draw circle border
     #draw.arc(((width - width/5, width - width/5), (width*9 + width/5, width*9 + width/5)),0,360,fill='blue', width = int(width/8))
@@ -289,7 +289,7 @@ def qr_image(data, width=600):
         text_width = font.getlength(partnum[2:])
 
     txtx = (int(width * 10) - text_width) / 2
-    txty = (int(width * 10)) / 2
+    txty = (int(width * 7.5)) / 2
     # draw part number text
     draw.text((txtx,txty),partnum[2:], "black", font)
     
@@ -298,10 +298,11 @@ def qr_image(data, width=600):
     qrcode = segno.make('HTTPS://BLDN.APP/' + partnum,micro=False,boost_error=False,error="L",mask=3)
     out = io.BytesIO()
     qrx, _ = qrcode.symbol_size(1,0)
-    qrcode.save(out, scale=width*2/qrx, kind="PNG", border=0)
+    qrcode.save(out, scale=width*3/qrx, kind="PNG", border=0)
     qrimg = Image.open(out)
-    img.paste(qrimg, box=(int(width*4),int(width*5.75)))
+    img.paste(qrimg, box=(int(width*3.5),int(width*4.5)))
-
+    img = img.crop((width+int(width / 1.4)-int(width/8),width+int(width / 1.4)-int(width/8),img.size[0] - (width+int(width / 1.4)-int(width/8)), img.size[1] - (width+int(width / 1.4)-int(width/8))  ))
+    img = img.resize((1200, 1200), Image.LANCZOS) # 1200 dpi
     
     return img
 

led_control.py

@@ -34,6 +34,10 @@ class LEDSystem():
     animation_time = 0
     start = uptime()
     solid_controllers = None
+    showtoggle = False
+    clearshow = -1
+    showring = -1 
+    solidanimcount = 0
     
     def __init__(self):
         self.start = uptime()
@@ -264,7 +268,41 @@ class LEDSystem():
         offset = len(self.controllers)
         for x in range(len(self.solid_controllers)):
             ctrl = self.solid_controllers[x]
-            self.sender[ctrl[0]+1].dmx_data = list(ctrl[1]) * 170
+            data = list((0, 0, 0)) * 170
+            #print(len(data))
+            #print(len(list(ctrl[1]) * 170))
+            offset = 1 - (self.solidanimcount - int(self.solidanimcount))
+            #print("a", offset)
+            data[int(self.solidanimcount) * 3] = int(ctrl[1][0]*offset) 
+            data[int(self.solidanimcount) * 3 + 1] = int(ctrl[1][1]*offset)  
+            data[int(self.solidanimcount) * 3 + 2] = int(ctrl[1][2]*offset) 
+            offset = 1 - offset
+            #print("b", offset)
+            if int(self.solidanimcount) <= 168:
+                data[int(self.solidanimcount+1) * 3] = int(ctrl[1][0]*offset) 
+                data[int(self.solidanimcount+1) * 3 + 1] = int(ctrl[1][1]*offset)  
+                data[int(self.solidanimcount+1) * 3 + 2] = int(ctrl[1][2]*offset)
+
+            # if offset > 0.5: # onto next light more
+            #     # center +1
+            #     num = int(self.solidanimcount+1)
+            # else:
+            # num = int(self.solidanimcount)
+            # offset = 1 - offset
+            # if int(num) <= 168:
+            #     data[int(num+1) * 3] = int(ctrl[1][0]*offset/2) 
+            #     data[int(num+1) * 3 + 1] = int(ctrl[1][1]*offset/2)  
+            #     data[int(num+1) * 3 + 2] = int(ctrl[1][2]*offset/2)
+
+            # if int(num) >= 1:
+            #     data[int(num-1) * 3] = int(ctrl[1][0]*offset/2) 
+            #     data[int(num-1) * 3 + 1] = int(ctrl[1][1]*offset/2)  
+            #     data[int(num-1) * 3 + 2] = int(ctrl[1][2]*offset/2)
+
+            self.sender[ctrl[0]+1].dmx_data = data #list(ctrl[1]) * 170
+        self.solidanimcount += 0.15
+        if int(self.solidanimcount) >= 170:
+            self.solidanimcount = 0
 
 
         self.sender.flush()
@@ -333,6 +371,8 @@ class LEDSystem():
 
     def setmode(self, stmode, r=0,g=0,b=0):
         if stmode is not None:
+            if self.mode == "Show":
+                self.clearshow = self.showring
             if self.mode != stmode:
                 self.firstrun = True
 
@@ -363,6 +403,9 @@ class LEDSystem():
 
     def runmodes(self, ring = -1, arm_position = None):
         #fprint("Mode: " + str(self.mode))
+        if self.clearshow > -1:
+            self.setring(0,50,100,self.clearshow)
+            self.clearshow = -1
         if self.mode == "Startup":
             # loading animation. cable check
             if self.firstrun:
@@ -452,6 +495,22 @@ class LEDSystem():
                 self.setring(0,100,0,ring)
                 self.setmode("Idle")
         
+        elif self.mode == "Show":
+            if self.firstrun:
+                self.showring = ring
+                self.firstrun = False
+                self.showtoggle = False
+                self.changecount = self.animation_time / 2 # 100hz
+            if self.changecount > 0:
+                #self.changecount = self.fadeorder(self.rings[ring][2],self.rings[ring][3], self.changecount, 0,100,0)
+                if not self.showtoggle:
+                    self.changecount = self.fadeorder(self.rings[ring][2],self.rings[ring][2]+24, self.changecount, 0,100,0)
+                else:
+                    self.changecount = self.fadeorder(self.rings[ring][2],self.rings[ring][2]+24, self.changecount, 100,20,100)
+            else:
+                self.changecount = self.animation_time / 2 # 100hz
+                self.showtoggle = not self.showtoggle
+
         elif self.mode == "Idle":
             if self.firstrun:
                 self.firstrun = False
@@ -749,12 +808,13 @@ if __name__ == "__main__":
     import matplotlib.pyplot as plt
     ledsys = LEDSystem()
     ledsys.init()
-    cap = cv2.VideoCapture('output.mp4')
+    while True:
-    while cap.isOpened():
+        cap = cv2.VideoCapture('output.mp4')
-        ret, frame = cap.read()
+        while cap.isOpened():
-        if not ret:
+            ret, frame = cap.read()
-            break
+            if not ret:
-        ledsys.mapimage(frame, fps=60)
+                break
+            ledsys.mapimage(frame, fps=60)
 
     show = False
     ring = 1

led_demo.bat

@@ -0,0 +1 @@
+python led_control.py

process_video.py

@@ -4,39 +4,55 @@ import cv2
 import banner_ivu_export
 import numpy as np
 from util import fprint
+import requests
 
 class qr_reader():
     camera = None
     def __init__(self, ip, port):
         self.ip = ip
         self.port = port
+        self.url = "http://" + ip + ":" + str(port) + "/barcode"
         #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):
+    #         print(str(x) + " ", end="", flush=True)
+    #         imgtype, img = self.camera.read_img()
+                
+    #         if img is not None:    
+    #             #fprint(imgtype)
+    #             image_array = np.frombuffer(img, np.uint8)
+    #             img = cv2.imdecode(image_array, cv2.IMREAD_COLOR)
+    #             #cv2.imshow('Image', img)
+    #             #cv2.waitKey(1)
+    #             detect = cv2.QRCodeDetector()
+    #             value, points, straight_qrcode = detect.detectAndDecode(img)
+                
+    #             if value != "":
+    #                 self.camera.close()
+    #                 return value
+    #         else:
+    #             print("\nGot no image for " + str(x))
+
+    #     self.camera.close()
+    #     return False
     def read_qr(self, tries=1):
-        print("Trying " + str(tries) + " frames.")
+        try:
-        self.camera = banner_ivu_export.DriveImg(self.ip, self.port)
+            response = requests.get(self.url, timeout=tries * 15)
-        for x in range(tries):
+            response.raise_for_status()  # Raise an error for bad status codes
-            print(str(x) + " ", end="", flush=True)
+            print(response.text)  # Or handle the response as needed
-            imgtype, img = self.camera.read_img()
+            if len(response.text) < 8:
+                return False
+            return response.text
+        except requests.Timeout:
+            print(f'The request timed out after {tries * 15} seconds')
+        except requests.RequestException as e:
+            print(f'An error occurred: {e}')
         
-            if img is not None:    
-                #fprint(imgtype)
-                image_array = np.frombuffer(img, np.uint8)
-                img = cv2.imdecode(image_array, cv2.IMREAD_COLOR)
-                #cv2.imshow('Image', img)
-                #cv2.waitKey(1)
-                detect = cv2.QRCodeDetector()
-                value, points, straight_qrcode = detect.detectAndDecode(img)
-                
-                if value != "":
-                    self.camera.close()
-                    return value
-                    
-
-        self.camera.close()
         return False
     
-
 class video_streamer():
     camera = None
     def __init__(self, ip, port):

read_datasheet.py

@@ -17,6 +17,7 @@ import os
 import glob
 import sys
 from PIL import Image
+import segno
 
 def touch(path):
     with open(path, 'a'):
@@ -197,6 +198,18 @@ def parse(filename, output_dir, partnum, dstype, weburl, extra):
     else:
         img = None
 
+    fprint("Making QR code for part " + partnum)
+    partnumqr = partnum.replace(" ", "%20")
+    if dstype == "Alphawire":
+        partnumqr = "AW" + partnumqr
+    if dstype == "Belden":
+        partnumqr = "BL" + partnumqr
+    qrcode = segno.make('HTTPS://BLDN.APP/' + partnumqr,micro=False,boost_error=False,error="L",mask=3)
+    #out = io.BytesIO()
+    qrx, _ = qrcode.symbol_size(1,0)
+    qrcode.save(output_dir + "/qrcode.png", scale=500.0/qrx, kind="PNG", border=0, light="#00000000")
+    qrpath = weburl + find_file_noext(output_dir, prefix="qrcode")[0]
+
     # Table parsing and reordring
     tables = dict()
     torename = dict()
@@ -368,14 +381,15 @@ def parse(filename, output_dir, partnum, dstype, weburl, extra):
         output_table["brand"] = extra["brand"]
     else:
         output_table["brand"] = dstype
-
+    output_table["datasheet"] = weburl + "datasheet.pdf"
+    output_table["qrcode"] = qrpath 
     if img is not None:
         output_table["image"] = img
-        output_table["fullspecs"] = {"partnum": partnum, "id": id, "brand": output_table["brand"], "image": img, "datasheet": output_dir + "/datasheet.pdf", **tables}
+        output_table["fullspecs"] = {"partnum": partnum, "id": id, "brand": output_table["brand"], "image": img, "datasheet": weburl + "datasheet.pdf", "qrcode": qrpath, **tables}
-        output_table["searchspecs"] = {"partnum": partnum, "brand": output_table["brand"], "image": img, "datasheet": output_dir + "/datasheet.pdf", **flatten(tables)}
+        output_table["searchspecs"] = {"partnum": partnum, "brand": output_table["brand"], "image": img, "datasheet": weburl + "datasheet.pdf", "qrcode": qrpath, **flatten(tables)}
     else:
-        output_table["fullspecs"] = {"partnum": partnum, "id": id, "brand": output_table["brand"], "datasheet": output_dir + "/datasheet.pdf", **tables}
+        output_table["fullspecs"] = {"partnum": partnum, "id": id, "brand": output_table["brand"], "datasheet": weburl + "datasheet.pdf", "qrcode": qrpath, **tables}
-        output_table["searchspecs"] = {"partnum": partnum, "brand": output_table["brand"], "datasheet": output_dir + "/datasheet.pdf", **flatten(tables)}
+        output_table["searchspecs"] = {"partnum": partnum, "brand": output_table["brand"], "datasheet": weburl + "datasheet.pdf", "qrcode": qrpath, **flatten(tables)}
 
     if "short_description" in extra:
         output_table["short_description"] = extra["short_description"]

requirements.txt

@@ -5,7 +5,7 @@ pypdf2==2.12.1
 alive-progress
 requests
 math3d==4.0.0
-git+https://git.myitr.org/adeck/python-urx-getl-rt.git
+git+https://github.com/Byeongdulee/python-urx.git
 meilisearch
 pyyaml
 Flask
@@ -22,6 +22,7 @@ ghostscript
 pyzbar
 segno
 pyModbusTCP
+paho-mqtt
 
 # Development
 matplotlib

run.py

@@ -11,6 +11,7 @@ import multiprocessing
 from time import sleep
 from util import fprint
 from util import run_cmd
+from util import win32
 import sys
 import ur5_control
 from ur5_control import Rob
@@ -30,9 +31,13 @@ from search import JukeboxSearch
 from pyModbusTCP.client import ModbusClient
 from uptime import uptime
 import fileserver
+import paho.mqtt.client as mqtt
+import pickle
+import time
+import subprocess
 
 # set to false to run without real hardware for development
-real = True
+real = False
 skip_scanning = True
 
 mbconn = None
@@ -73,6 +78,19 @@ arm_position = (0,0,0,0,0,0)
 arm_position_process = None
 start_animation = False
 failcount = 0
+timecount = 0
+secondsclock = 0
+unacked_publish = set()
+mqttc = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2)
+counter_file = 'pick_count.txt'
+cycle_start_time = time.time()
+arm_distance = 0
+arm_distance_old = 0
+arm_distance_total = 0
+kill_ssh = False
+mqttc.user_data_set(unacked_publish)
+spot = -1
+placed = 0
 
 def arm_start_callback(res):
     fprint("Arm action complete.")
@@ -120,6 +138,38 @@ def send_data(type, call, data, client_id="*"):
     out["data"] = data
     to_server_queue.put((client_id, json.dumps(out)))    
 
+def initialize_counter():
+    global counter
+    try:
+        with open(counter_file, 'rb') as file:
+            counter = int(file.readline().decode())
+            fprint("Read count at " + str(counter))
+            if not isinstance(counter, int):
+                raise ValueError("Counter is not an integer")
+                
+    except (FileNotFoundError, ValueError) as e:
+        counter = 101
+        fprint("Error. Resetting count." + str(e))
+        fprint(counter)
+        save_counter()
+
+    fprint(counter)
+
+# Save the counter to the file
+def save_counter():
+    global counter
+    fprint(counter)
+    with open(counter_file, 'wb') as file:
+        file.write(str(counter).encode())
+
+# Increment the counter
+def increment_counter():
+    global counter
+    fprint(counter)
+    fprint("Setting count to " + str(counter + 1))
+    counter = counter + 1
+    save_counter()
+
 def check_server():
         #print("HI")
     global cable_list
@@ -214,6 +264,10 @@ def check_server():
 
                     case "ping":
                         fprint("Pong!!!")
+
+                    case "mode":
+                        if call == "request":
+                            send_data("mode", "send", "{\"mode\": \"" + mode + "\" }")
                     # Lucas' notes
                     # Add a ping pong :) response/handler
                     # Add a get cable response/handler
@@ -331,10 +385,16 @@ def check_server():
                                 mainloop_get.put(("pickup", data["position"]))
                             elif "tray" in data:
                                 fprint("Adding tray return to dispense queue")
-                                mainloop_get.put(("return", data["tray"]))
+                                mainloop_get.put(("returnCheck", 0))
+                                #mainloop_get.put(("return", data["tray"]))
                             else:
                                 fprint("Invalid data.")
+                        elif call == 'request':
+                            if "position" in data:
+                                mainloop_get.put(("show", data["position"]))
                     
+                    case "shutdown":
+                        mainloop_get.put(("shutdown", 0))
                     case _:
                         fprint("Unknown/unimplemented data type: " + decoded["type"])
             except Exception as e:
@@ -422,8 +482,9 @@ def setup_server(pool, manager):
     if sensor_ready is False:
         fprint("waiting for " + "Sensor Initialization" + " to complete...", sendqueue=to_server_queue)
         global mbconn
-        mbconn = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=True)
+        if real:
-        get_sensors()
+            mbconn = ModbusClient(host="192.168.1.20", port=502, auto_open=False, auto_close=False)
+            get_sensors()
     fprint("Sensors initialized.", sendqueue=to_server_queue)
 
     if camera_ready is False:
@@ -448,20 +509,47 @@ def setup_server(pool, manager):
     fprint("Starting websocket server...", sendqueue=to_server_queue)
     websocket_process = server.start_websocket_server(to_server_queue, from_server_queue)
     
-    fprint("Starting image file server...", sendqueue=to_server_queue)
+    fprint("Starting file server...", sendqueue=to_server_queue)
     image_server_process = Process(target=fileserver.run_server, args=(config["cables"]["port"], config["cables"]["directory"]))
     image_server_process.start()
+
+    if config["mqtt"]["enabled"]:
+        global mqttc
+        global unacked_publish
+        mqttc.on_publish = on_publish
+
+        mqttc.user_data_set(unacked_publish)
+        mqttc.connect(config["mqtt"]["server"])
+        mqttc.loop_start()
+    
+    
+
     return True
 
+def mqtt_send(msg, name):
+    global config
+    if config["mqtt"]["enabled"]:
+        global mqttc
+        global unacked_publish
+        msg_info = mqttc.publish("jukebox/" + name, msg, qos=1)
+        unacked_publish.add(msg_info.mid)
+        while len(unacked_publish):
+            sleep(0.01)
+
+            # Due to race-condition described above, the following way to wait for all publish is safer
+        msg_info.wait_for_publish()
+        
 def handle_error(error):
 	print(error, flush=True)
 
 def get_sensors():
     global mbconn
     global sensors
-    oldsens = sensors
     #print("Reading sensors")
-    #mbconn.open()
+    if not real:
+        return -1
+    if not mbconn.is_open:
+        mbconn.open()
     """
     port 1: 256
     port 2: 272
@@ -473,41 +561,83 @@ def get_sensors():
     port 8: 368
     
     """
-    out = list()
     if real:
-        for reg in [352, 288, 304, 368]:
+        sens = [320, 336, 352, 368]
+        for idx in range(len(sens)):
+            reg = sens[idx]
             val = mbconn.read_holding_registers(reg)
+            #fprint("Sensor " + str(idx) + " = " + str(val))
             if val is not None:
                 val = val[0]
-                if val == 1:
+                if val == 1: # skip negative values
-                    out.append(1)
+                    sensors[idx] += 1
-                else:
+                elif val == 0:
-                    out.append(0)
+                    sensors[idx] -= 1
-    else:
+                
-        out = [0, 0, 0, 0]
+        
+                    
+
+    else:
+        sensors = [-10, -10, -10, -10]
 
-    if len(out) != 4:
-        return -1
-    sensors = out
     #fprint("Values: " + str(sensors))
     #mbconn.close()
-    for x in range(len(oldsens)):
+    for x in range(len(sensors)):
-        if oldsens[x] == 0 and out[x] == 1:
+        if sensors[x] > 10:
-            # cable newly detected on tray
+            sensors[x] = 10
-            fprint("Precense detected: slot " + str(x))
+            
-            return x
+        if sensors[x] < -10:
+            sensors[x] = -10
         
     return -1
+
 def get_open_spot(sensordata):
-    for x in range(len(sensordata)):
+    for x in range(len(sensors)):
-        sens = sensordata[x]
+        sens = sensors[x]
-        if not sens:
+        if sens <= -5:
+            print("Open spot: " + str(x))
             return x
         
     # if we get here, every spot is full
-        
+    fprint("No spots empty")
     return False
     
+def get_full_spot(sensordata):
+    for x in range(len(sensors)):
+        sens = sensors[x]
+        if sens >= 3:
+            print("Full spot: " + str(x))
+            return x
+        
+    # if we get here, every spot is empty
+    fprint("No spots full")
+    return False
+
+def get_spot(sensordata, idx):
+    if sensordata[idx] >= 3:
+        return True
+    elif sensordata[idx] <= -5:
+        return False
+    else:
+        return False
+    
+def on_publish(client, userdata, mid, reason_code, properties):
+    # reason_code and properties will only be present in MQTTv5. It's always unset in MQTTv3
+    try:
+        userdata.remove(mid)
+    except KeyError:
+        print("on_publish() is called with a mid not present in unacked_publish")
+        print("This is due to an unavoidable race-condition:")
+        print("* publish() return the mid of the message sent.")
+        print("* mid from publish() is added to unacked_publish by the main thread")
+        print("* on_publish() is called by the loop_start thread")
+        print("While unlikely (because on_publish() will be called after a network round-trip),")
+        print(" this is a race-condition that COULD happen")
+        print("")
+        print("The best solution to avoid race-condition is using the msg_info from publish()")
+        print("We could also try using a list of acknowledged mid rather than removing from pending list,")
+        print("but remember that mid could be re-used !")
+
 
 def mainloop_server(pool, manager):
     # NON-blocking loop
@@ -531,6 +661,7 @@ def mainloop_server(pool, manager):
     global mainloop_get
     global cable_list_state
     global scan_value
+    global sensors
     global oldmode
     global arm_updates
     global animation_wait
@@ -538,12 +669,23 @@ def mainloop_server(pool, manager):
     global arm_position_process
     global start_animation
     global failcount
+    global timecount
+    global secondsclock
+    global cycle_start_time
+    global arm_distance
+    global arm_distance_old
+    global arm_distance_total
+    global placed
+    global spot
 
     if mode != oldmode:
         print(" ***** Running mode:", mode, "***** ")
+        send_data("mode", "send", "{\"mode\": \"" + mode + "\" }")
         oldmode = mode
+        mqtt_send(mode, "mode")
         if mode == "Startup": # very first loop
             pass
+            initialize_counter()
 
     if killme.value > 0:
         killall()
@@ -551,20 +693,28 @@ def mainloop_server(pool, manager):
     # check for messages
     check_server()
 
+    
     # do every loop!
     checkpoint = None
+    val = None
     if not arm_updates.empty():
         val = arm_updates.get()
         
         if isinstance(val, tuple):
             arm_position = val
+            if arm_distance_old == 0:
+                arm_distance_old = val[0] + val[1] + val[2]
+                arm_distance = 0
+            else:
+                arm_distance += val[0] + val[1] + val[2] - arm_distance_old
+                arm_distance_old = val[0] + val[1] + val[2] 
         else:
             print("Arm queue message " + str(val))
             checkpoint = val
             print(ring_animation, animation_wait, ledsys.mode, arm_position)
             
 
-    if start_animation:
+    if start_animation and real:
         # animation start requested
         # may not be immediate
         if ring_animation is not None:
@@ -594,12 +744,28 @@ def mainloop_server(pool, manager):
     
     else:
         # no new animation
-        if ring_animation is not None:
+        if ring_animation is not None and real:
             ledsys.mainloop(None, ring_animation, arm_position=arm_position)
             
-        else:
+        elif real:
-            pass
+            ledsys.mainloop(None, -1, arm_position=arm_position)
 
+    # every 1 second
+    if secondsclock >= config["core"]["loopspeed"] / 2:
+        secondsclock = 1
+        arm_distance_total += abs(arm_distance)
+        if abs(arm_distance) < 0.001:
+            arm_distance = 0.0
+            
+        mqtt_send("{\"value\": " + str(abs(arm_distance)) + " }", "arm_speed")
+        mqtt_send("{\"value\": " + str(abs(arm_distance_total)) + " }", "arm_distance") 
+        arm_distance_old = 0 # reset counter
+        get_sensors()
+
+
+
+    else:
+        secondsclock += 1
     # if start_animation is False and ring_animation is not None and ledsys.mode != "Idle" and real:
     #     ledsys.mainloop(None, ring_animation, arm_position=arm_position)
 
@@ -654,8 +820,8 @@ def mainloop_server(pool, manager):
                 global scan_value
                 if scan_value is False:
                     cable_list.append(scan_value)
-                elif scan_value.find("bldn.app/") > -1:
+                elif scan_value.upper().find("BLDN.APP/") > -1:
-                    scan_value = scan_value[scan_value.find("bldn.app/")+9:]
+                    scan_value = scan_value[scan_value.upper().find("BLDN.APP/")+9:]
                 else:
                     cable_list.append(scan_value)
                 fprint(scan_value)
@@ -760,63 +926,24 @@ def mainloop_server(pool, manager):
                     pass
 
             
-    if mode == "Idle":
-        # do nothing
-        if arm_ready is False:
-            pass
     
-        else:
-            global mainloop_get
-            #print("Checking sensors..")
-            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():
-                fprint("Movement requested. Keep clear of the machine!")
-                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
-                            if real:
-                                animation_wait = False
-                                ring_animation = get_cable
-                                start_animation = True
-                                led_set_mode = "GrabAA"
-                                pool.apply_async(ur5_control.holder_to_tray, (arm, arm_updates, get_cable, spot), callback=arm_start_callback, error_callback=handle_error)
-                            else:
-                                arm_ready = True
-                            fprint("Getting cable at position " + str(get_cable))
-                            mode = "Pickup"
-                            cable_list_state[get_cable] = False # mark as removed
-                            get_sensors()
-
-                    if action == "return":
-                        arm_ready = False
-                        fprint("Returning cable from tray position " + str(get_cable))
-                        if real:
-                            failcount = 0
-                            pool.apply_async(ur5_control.tray_to_camera, (arm, arm_updates, get_cable), callback=arm_start_callback, error_callback=handle_error)
-                        else:
-                            arm_ready = True
-                        mode = "ReturnC"
-            else:
-                # LED idle anim
-                pass
 
     if mode == "Pickup":
         # complete
         if arm_ready == True:
             mode = "Idle"
-            
+            if not real:
+                sleep(9)
+            #global sensors
+            if placed > 5:
+                # success
+                mqtt_send("{\"value\": " + str(1) + " }", "pick_count_success")
+            mqtt_send("{\"value\": " + str(int(time.time() * 1000) - cycle_start_time) + " }", "cycle_time")
         else:
             # getting cable and bringing to tray
             # led animation
+            if get_spot(sensors, spot):
+                placed += 1
             if ledsys.mode == "Idle" and led_set_mode != "GrabAA":
                 animation_wait = True
                 start_animation = True
@@ -834,7 +961,8 @@ def mainloop_server(pool, manager):
                 start_animation = True
                 ring_animation = 49
                 led_set_mode = "Camera"
-                pool.apply_async(camera.read_qr, (50,), callback=camera_start_callback, error_callback=handle_error)
+                timecount = 0
+                pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
             else:
                 camera_ready = True
                 scan_value = "10GXS13"
@@ -848,55 +976,76 @@ def mainloop_server(pool, manager):
         if camera_ready == True:
             if scan_value is False:
                 # unable to scan ???? not good
-                if failcount > 15:
+                if failcount > 30:
                     mode = "Idle"
                     fprint("Giving up scanning cable.")
                     failcount = 0
+                    timecount = 0
+                    arm_ready = True
                 else:
                     fprint("Unable to scan cable. Gonna retry.")
                     camera_ready = False
                     #mode = "Idle"
                     failcount += 1
-
+                    timecount = 0
                     pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
 
-            elif scan_value.find("bldn.app/") > -1:
+            elif scan_value.upper().find("BLDN.APP/") > -1:
-                scan_value = scan_value[scan_value.find("bldn.app/")+9:]
+                scan_value = scan_value[scan_value.upper().find("BLDN.APP/")+9:].strip("\r\n\t ")
+            
                 
-                fprint("Got cable: " + str(scan_value))
                 if scan_value[0:2] == "BL" or scan_value[0:2] == "AW":
                     scan_value = scan_value[2:]
+                #print(cable_list)
+                fprint("Got cable: " + repr(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
-                        if real:
-                            animation_wait = True
-                            ring_animation = idx
-                            led_set_mode = "GrabC"
-                            start_animation = True
-                            pool.apply_async(ur5_control.camera_to_holder, (arm, arm_updates, idx), callback=arm_start_callback, error_callback=handle_error)
-                        else:
-                            arm_ready = True
-                        mode = "Return"
-                        break
-                    elif cable == scan_value and cable_list_state[idx] == True:
-                        fprint("WARNING: Holder still marked as occupied!")
-                        arm_ready = False
-                        if real:
-                            animation_wait = True
-                            ring_animation = idx
-                            led_set_mode = "GrabC"
-                            start_animation = True
-                            pool.apply_async(ur5_control.camera_to_holder, (arm, arm_updates, idx), callback=arm_start_callback, error_callback=handle_error)
-                        else:
-                            arm_ready = True
-                        mode = "Return"
-                        break
-                if mode == "Scan":
-                    mode = "Idle"
                     
+                    if cable is not False and cable.find(scan_value) == 0 and len(scan_value) == len(cable):
+                        if cable_list_state[idx] == False:
+                            cable_list_state[idx] = True # mark cable as returned
+                            arm_ready = False
+                            if real:
+                                animation_wait = True
+                                ring_animation = idx
+                                led_set_mode = "GrabC"
+                                start_animation = True
+                                fprint("Returning to spot " + str(idx))
+                                pool.apply_async(ur5_control.camera_to_holder, (arm, arm_updates, idx), callback=arm_start_callback, error_callback=handle_error)
+                            else:
+                                arm_ready = True
+                            mode = "Return"
+                            break
+                        else:
+                            print(cable_list)
+                            print("Length:", len(cable_list))
+                            for idx in range(len(cable_list)-1,-1,-1):
+                                cable = cable_list[idx]
+                                if cable is False:
+                                    if real:
+                                        animation_wait = True
+                                        ring_animation = idx
+                                        led_set_mode = "GrabC"
+                                        start_animation = True
+                                        cable_list_state[idx] = True
+                                        fprint("Returning to unused spot " + str(idx))
+                                        pool.apply_async(ur5_control.camera_to_holder, (arm, arm_updates, idx), callback=arm_start_callback, error_callback=handle_error)
+                                    else:
+                                        arm_ready = True
+                        
+                            
+                if mode == "Scan":
+                    fprint("Unable to match cable.")
+                    mode = "Idle"
+        else: # camera not ready
+            timecount += 1
+            if timecount > 180:
+                print("Camera timeout reached.")
+                timecount = 0
+                camera_ready = True
+                arm_ready = True
+                mode = "Idle"
             
 
     if mode == "Return":
@@ -910,9 +1059,104 @@ def mainloop_server(pool, manager):
             # led animation
             pass
             
+    if mode == "IdleWait":
+        if arm_ready is True:
+            mode = "Idle"
+            
+    if mode == "Idle":
+        # do nothing
+        if arm_ready is False:
+            mode = "IdleWait"
+
+        else:
+            global mainloop_get
             
             
+            if not mainloop_get.empty():
                 
+                action, get_cable = mainloop_get.get()
+                if action == "show":
+                    animation_wait = False
+                    ring_animation = get_cable
+                    start_animation = True
+                    led_set_mode = "Show"
+                    fprint("Showing cable at position " + str(get_cable))
+                elif action == "shutdown":
+                    fprint("SHUTTING DOWN!!")
+                    pool.apply_async(ur5_control.move_to_packup, (arm,), callback=arm_start_callback, error_callback=handle_error)
+                    sleep(30)
+                    killme.set(1)
+                elif action == "returnCheck":
+                    print("Checking sensors..")
+                    if real:
+                        newtube = get_full_spot(sensors)
+                    else:
+                        newtube = -1
+                    if newtube is not False and newtube >= 0:
+                        # need to return a cable
+                        mainloop_get.put(("return", newtube))
+                        mainloop_get.put(("returnCheck", 0))
+                else:
+                    fprint("Movement requested. Keep clear of the machine!")
+                    
+                    placed = 0
+                    arm_distance_total = 0
+                    #mqtt_send("{\"value\": " + str(time.time() * 1000) + " }", "cycle_start")
+                    cycle_start_time = int(time.time() * 1000)
+                    increment_counter()
+                    mqtt_send("{\"value\": " + str(1) + " }", "pick_count_total")
+                    if get_cable > -1:
+                        
+                        
+                        if action == "pickup":
+                            spot = get_open_spot(sensors)
+                            
+                            if spot is not False:
+                                arm_ready = False
+                                if real:
+                                    animation_wait = False
+                                    ring_animation = get_cable
+                                    start_animation = True
+                                    led_set_mode = "GrabAA"
+                                    pool.apply_async(ur5_control.holder_to_tray, (arm, arm_updates, get_cable, spot), callback=arm_start_callback, error_callback=handle_error)
+                                else:
+                                    arm_ready = True
+                                fprint("Getting cable at position " + str(get_cable))
+                                mode = "Pickup"
+                                cable_list_state[get_cable] = False # mark as removed
+
+
+                        if action == "return":
+                            arm_ready = False
+                            fprint("Returning cable from tray position " + str(get_cable))
+                            if real:
+                                failcount = 0
+                                pool.apply_async(ur5_control.tray_to_camera, (arm, arm_updates, get_cable), callback=arm_start_callback, error_callback=handle_error)
+                            else:
+                                arm_ready = True
+                            mode = "ReturnC"
+            else:
+                # LED idle anim
+                pass
+    
+        
+def ping(host):
+        #Returns True if host (str) responds to a ping request.
+
+        # 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 run_loading_app():
     
@@ -924,45 +1168,83 @@ def run_loading_app():
     app.run(debug=True, use_reloader=False, port=7000)
 
 def setup_client(pool):
-    # Windows client setup
-    fprint("Opening browser...")
-    firefox = webdriver.Firefox()
-    firefox.fullscreen_window()
     global config
     global vm_ready
     global serverproc
-    # Open loading wepage
-    p = Process(target=run_loading_app)
-    p.start()
-
-    
-    firefox.get('http://localhost:7000')
-    
-    # start Linux server VM
     if config["core"]["server"] == "Hyper-V":
-        run_cmd("Start-VM -Name Jukebox*") # any and all VMs starting with "Jukebox"
+        run_cmd("Restart-VM -Name Jukebox*") # any and all VMs starting with "Jukebox"
+        sleep(2)
+    fprint("Waiting for VM to start...")
+    while not ping("192.168.1.25"):
+        sleep(0.25)
 
-    # Wait for VM to start and be reachable over the network
+    fprint("VM online.")
-    serverproc = Process(target=start_client_socket)
+    sleep(2)
-    serverproc.start()
+    # Windows client setup
+    fprint("Running full jukebox control system...")
+    jb = subprocess.Popen("ssh root@192.168.1.25 -t -- /root/jukebox-software/run.sh".split(' '), stdout=subprocess.PIPE, stderr=subprocess.STDOUT, bufsize=1, universal_newlines=True, encoding='utf-8')
+    while True:
+        if jb.poll() is not None:
+            break
+        line = jb.stdout.readline()   # Alternatively proc.stdout.read(1024)
+        if len(line) == 0: # program end
+            break
+        print(line, end='')
+        if line.find("Running mode: Idle") > 0:
+            # Jukebox started
+            break # continue with program
     
-    pool.apply_async(check_server_online, (config["core"]["serverip"],config["core"]["clientip"]), callback=vm_start_callback)
+    if jb.poll() is None:
 
-    #wait_for(vm_ready, "VM Startup")
+        fprint("Opening browser...")
     
-    #global vm_ready
+        firefox = webdriver.Firefox()
-    if vm_ready is False:
+        firefox.fullscreen_window()
-        fprint("waiting for " + "VM Startup" + " to complete...")
+
-        while vm_ready is False:
+    
-            sleep(0.1)
+        # firefox.get('http://localhost:7000')
-    p.terminate()
+    
-    firefox.get("http://" + config["core"]["serverip"] + ":8000")
+
-    return True
+        firefox.get('http://192.168.1.25:3000')
+        global kill_ssh
+        while True:
+            if jb.poll() is not None:
+                break
+            line = jb.stdout.readline()   # Alternatively proc.stdout.read(1024)
+            if len(line) == 0: # program end
+                break
+            print(line, end='')
+            if kill_ssh is True:
+                break
+
+        
+        firefox.close()
+
+    jb.terminate()
+    run_cmd("Stop-VM -Name Jukebox*")
+    sleep(2)
+    killall()
+        
+    #firefox.execute_script('document.body.style.MozTransform = "scale(0.80)";')
+    #firefox.execute_script('document.body.style.MozTransformOrigin = "0 0";')
+    #firefox.execute_script("document.body.style.zoom='80%'")
+    # import time
+    # while True:
+    #     time.sleep(2)  # Wait for a given interval
+    #     logs = firefox.get_log('browser')
+    #     for entry in logs:
+    #         if "WebSocket connection" in entry['message'] or "ERR_" in entry['message'] or "Failed to connect" in entry['message'] or "failed to connect" in entry['message']:
+    #             print(f"Error detected in console: {entry['message']}")
+    #             firefox.refresh()  # Refresh the page on error
+    #         # else:
+    #         #     break  # Exit the loop or continue depending on your logic
+
+        #return True
     
 
 def mainloop_client(pool):
     sleep(0.1)
-
+    killall()
     # listen for & act on commands from VM, if needed
     # mainly just shut down, possibly connect to wifi or something
 
@@ -993,9 +1275,13 @@ def killall():
     
 def killall_signal(a, b):
     global config
-    if config["core"]["server"] == "Hyper-V":
+    #if config["core"]["server"] == "Hyper-V":
-        run_cmd("Stop-VM -Name Jukebox*") # any and all VMs starting with "Jukebox"
+        #run_cmd("Stop-VM -Name Jukebox*") # any and all VMs starting with "Jukebox"
-    killall()
+    if config["core"]["mode"] == "winclient":
+        global kill_ssh
+        kill_ssh = True
+    else:
+        killall()
 
 def error(msg, *args):
     return multiprocessing.get_logger().error(msg, *args)

run.sh

@@ -0,0 +1,5 @@
+#!/bin/sh
+
+pkill -9 python # kill any old processes
+cd /root/jukebox-software
+python run.py

server.py

@@ -83,7 +83,7 @@ async def send_messages(to_server_queue):
         await asyncio.sleep(0.001)
 
 def websocket_server(to_server_queue, from_server_queue):
-    start_server = websockets.serve(lambda ws, path: handler(ws, path, to_server_queue, from_server_queue), "localhost", 9000)
+    start_server = websockets.serve(lambda ws, path: handler(ws, path, to_server_queue, from_server_queue), "0.0.0.0", 9000)
 
     asyncio.get_event_loop().run_until_complete(start_server)
     asyncio.get_event_loop().create_task(send_messages(to_server_queue))

tempCodeRunnerFile.py

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

ur5_control.py

@@ -261,11 +261,12 @@ def move_to_home(robot, keep_flip=False, speed=2):
         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
+    rob.movej(offset_gripper_angle(robot, *(-0.18, -0.108, 0.25), flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position
 
     return True
 
 def move_to_packup(robot, speed=0.25):
+    robot = connect(robot)
     rob = robot.robot
     
     # known good starting point to reach store position
@@ -580,15 +581,17 @@ def tray_routine(robot, slot=0, pick_up=True):
 
    # Positions for each slot
     slot_distance = .052
-    slot_height = -.015-.0095
+    slot_height = -.015-.0095+0.007 # add 7mm for shim
-    first_slot = -0.3084+0.02
+    first_slot = -0.3084+0.01+0.003 # add 3mm for tray adjust
     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)
+    if pick_up:
+        open_gripper()
+    rob.movel(slot_position[slot], vel=0.2, acc=1)
 
     # Place/Grab the tube
     if pick_up:
@@ -679,13 +682,48 @@ def camera_to_holder(robot, pos_updates, holder_index, verbose=False):
     drop_off_holder(robot, pos_updates, holder_index)
     
 
+# def open_gripper():
+#     fprint("Opening gripper")
+#     c = ModbusClient(host="192.168.1.21", port=502, auto_open=False, auto_close=False)
+#     c.open()
+#     while not c.is_open:
+#         time.sleep(0.01)
+
+#     c.write_single_register(112, 0b0)
+#     # c.write_single_register(435, 0b10000000)
+#     time.sleep(0.5)
+#     # 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=False, auto_close=False)
+#     c.open()
+#     while not c.is_open:
+#         time.sleep(0.01)
+    
+#     c.write_single_register(435, 0b00000000)
+#     # c.write_single_register(112, 0b1)
+#     time.sleep(0.5)
+#     # c.write_single_register(435, 0b00000000)
+#     c.write_single_register(112, 0b1)
+#     time.sleep(0.5)
+#     c.close()
+#     time.sleep(0.2)
+#     #
+    
+
 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)
-    time.sleep(0.5)
+
     c.write_single_register(112, 0b0)
     c.write_single_register(435, 0b10000000)
     time.sleep(0.5)
@@ -698,35 +736,37 @@ def close_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)
-    time.sleep(0.5)
+
     c.write_single_register(435, 0b00000000)
     c.write_single_register(112, 0b1)
     time.sleep(0.5)
     c.close()
     #
 
+
 def get_position_thread(robot, pos_updates):
-    robot = connect(robot)
+    try:
-    rob = robot.robot
+        robot = connect(robot)
-    oldvals = rob.getl()
+        rob = robot.robot
-    deltavals = [0,0,0]
+        oldvals = rob.getl()
-    import uptime
+        deltavals = [0,0,0]
-    t = 0.01
+        import uptime
-    count = 0
+        t = 0.01
-    while True:
+        count = 0
-        start = uptime.uptime()
+        while True:
-        if pos_updates.qsize() < 2:
+            start = uptime.uptime()
-            vals = rob.getl()
+            if pos_updates.qsize() < 2:
-            if vals != oldvals:
+                vals = rob.getl()
-                if pos_updates is not None:
+                if vals != oldvals:
-                    pos_updates.put(tuple(oldvals))
+                    if pos_updates is not None:
+                        pos_updates.put(tuple(oldvals))
                 #time.sleep(0.01)
                 # deltavals = list()
                 # deltavals.append(vals[0]-oldvals[0])
                 # deltavals.append(vals[1]-oldvals[1])
                 # deltavals.append(vals[2]-oldvals[2])
                 # count = 0
-                oldvals = vals
+                    oldvals = vals
                 
             # else:
             #     count += 0.2
@@ -736,9 +776,10 @@ def get_position_thread(robot, pos_updates):
             #         tmpvals[1] = oldvals[1] + deltavals[1]*count
             #         tmpvals[2] = oldvals[2] + deltavals[2]*count
             #         pos_updates.put(tuple(tmpvals))
-        while start + t > uptime.uptime():
+            while start + t > uptime.uptime():
-            time.sleep(0.0001)
+                time.sleep(0.0001)
-                
+    except:
+        pass
 
 if __name__ == "__main__":
     
@@ -756,9 +797,9 @@ if __name__ == "__main__":
     rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously
 
 
-    move_to_packup(robot)
+    #move_to_packup(robot)
-    
+    move_to_home(robot)
-    # pick_up_holder(robot, 2)
+    pick_up_holder(robot, None, 8)
     #drop_off_tray(robot, 0)
     # drop_off_tray(robot, 1)
     # drop_off_tray(robot, 2)

util.py

@@ -26,8 +26,8 @@ if win32:
     #if not getattr(sys, "frozen", False):
     startupinfo.dwFlags |= subprocess.STARTF_USESHOWWINDOW # hide powershell window
     res = subprocess.check_output(["WMIC", "ComputerSystem", "GET", "UserName"], universal_newlines=True, startupinfo=startupinfo)
-    _, username = res.strip().rsplit("\n", 1)
+    # _, username = res.strip().rsplit("\n", 1)
-    userid, sysdom = username.rsplit("\\", 1)
+    # userid, sysdom = username.rsplit("\\", 1)
 
 if linux or macos:
     sysid = hex(uuid.getnode())