fixed i/o lock on global variable

Robot/UR/URModbusServer.py

@@ -67,7 +67,7 @@ class URModbusServer:
 
         if (packet is None) or (packet_signs is None):
             time.sleep(0.01)
-            print(f"[Angles] Modbus Error: retrying")
+            print(f"[Angles] Modbus Error #{tries}: retrying")
             return self.get_joint_angles(tries+1)
         else:
             base = self._format_sign(packet[9:11], packet_signs[9:11])
@@ -96,7 +96,7 @@ class URModbusServer:
 
         if (packet is None):
             time.sleep(0.01)
-            print(f"[Speeds] Modbus Error: retrying")
+            print(f"[Speeds] Modbus Error#{tries}: retrying")
             return self.get_joint_speeds(tries+1)
         else:
             base = self._format(packet[9:11]) / 1000

URSentry.py

@@ -200,7 +200,7 @@ class URSentry:
 
         # Deadzones where we still consider the target in the middle, to avoid jittering
         horizontal_dead_zone_radius = 0.1
-        vertical_dead_zone_radius = 10.08
+        vertical_dead_zone_radius = 0.08
 
         # Speeds for the base and neck joints
         base_max_speed = 1.5

main.py

@@ -24,15 +24,17 @@ class CameraJoystick(threading.Thread):
         self.keep_running = True
 
     def run(self):
+        global current_joystick
         while self.keep_running:
+            time.sleep(0.0001)
             q_val = []
             try:
                 q_val = q.get_nowait()
             except queue.Empty:
                 pass
             #print(bb)
-            bb = b.get_joystick_position_from_new_set_of_bboxes(q_val)
-            self.joystick_queue.put(bb)
+            current_joystick = b.get_joystick_position_from_new_set_of_bboxes(q_val)
+            #self.joystick_queue.put(bb)
 
     def stop(self):
         self.keep_running = False
@@ -58,8 +60,8 @@ class JoystickScheduler(threading.Thread):
 
 camera_joystick = CameraJoystick(joystick_queue)
 camera_joystick.start()
-joystick_scheduler = JoystickScheduler(joystick_queue)
-joystick_scheduler.start()
+# joystick_scheduler = JoystickScheduler(joystick_queue)
+# joystick_scheduler.start()
 
 ur.initialize_pose()
 
@@ -70,5 +72,5 @@ while True:
         time.sleep(0.2)
     except KeyboardInterrupt:
         camera_joystick.stop()
-        joystick_scheduler.stop()
+        # joystick_scheduler.stop()
         break