From 9b8f4d60c0a20a1a454646aa83f9d348f6f085fa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tom=C3=A1s=20L?= <37879596+Tomz295@users.noreply.github.com> Date: Fri, 19 Jul 2024 15:48:42 -0400 Subject: [PATCH] V1.0.0 sentry follows properly --- BBoxProcessor.py | 6 +++--- Communication/ModbusTCP.py | 8 ++++---- Robot/UR/URModbusServer.py | 10 ++++------ URSentry.py | 9 +++++---- main.py | 9 +++++++-- 5 files changed, 23 insertions(+), 19 deletions(-) diff --git a/BBoxProcessor.py b/BBoxProcessor.py index a21961f..c8ebd8b 100644 --- a/BBoxProcessor.py +++ b/BBoxProcessor.py @@ -14,7 +14,7 @@ class BBoxProcessor: """ new_boxes = self.process_next(new_boxes) closest_box = self.get_box_closest_to_center(new_boxes) - if closest_box is None: + if closest_box == []: return [0, 0] print("Closest box: ", closest_box, "All: ", new_boxes) return self.get_normalized_box_position(closest_box) @@ -49,7 +49,7 @@ class BBoxProcessor: closest = None for new_box in new_boxes: new_distance = self.bbox_distance(box, new_box) - if new_distance < min_distance and new_distance < distance: + if (new_distance < min_distance and new_distance < distance): distance = new_distance closest = new_box @@ -58,7 +58,7 @@ class BBoxProcessor: and timestamp < self.last_timestamps[index] + time_to_live ): new_boxes_list.append(box) - new_timestamps_list.append(timestamp) + new_timestamps_list.append(self.last_timestamps[index]) self.last_boxes = new_boxes_list self.last_timestamps = new_timestamps_list diff --git a/Communication/ModbusTCP.py b/Communication/ModbusTCP.py index cee7559..3308595 100644 --- a/Communication/ModbusTCP.py +++ b/Communication/ModbusTCP.py @@ -205,19 +205,19 @@ class ModbusTCP: if mbap[0] != self.__transaction_id: print("Modbus: Transaction ID mismatch" - "\n - Send: {} \n - Response: {}".format(self.__transaction_id, mbap[0])) + "\t - Send: {} \t - Response: {}".format(self.__transaction_id, mbap[0])) return True elif mbap[1] != self.__protocol_id: print("Modbus: Protocol ID mismatch" - "\n - Send: {} \n - Response: {}".format(self.__protocol_id, mbap[1])) + "\t - Send: {} \t - Response: {}".format(self.__protocol_id, mbap[1])) return True elif mbap[3] != self.__unit_id: print("Modbus: Unit ID mismatch" - "\n - Send: {} \n - Response: {}".format(self.__unit_id, mbap[3])) + "\t - Send: {} \t - Response: {}".format(self.__unit_id, mbap[3])) return True elif mbap[2] != len(response[6:]): print("Modbus: Length mismatch" - "\n - Length: {} \n - Remaining: {}".format(mbap[2], len(response[6:]))) + "\t - Length: {} \t - Remaining: {}".format(mbap[2], len(response[6:]))) return True function_code = struct.unpack(">B", function_code) diff --git a/Robot/UR/URModbusServer.py b/Robot/UR/URModbusServer.py index c6367a7..a50ecf6 100644 --- a/Robot/UR/URModbusServer.py +++ b/Robot/UR/URModbusServer.py @@ -66,11 +66,10 @@ class URModbusServer: packet_signs = self.modbusTCP.read_holding_registers(320, quantity=6) if (packet is None) or (packet_signs is None): - time.sleep(0.01) + time.sleep(0.05) print(f"Modbus Error #{tries}: retrying") return self.get_joint_angles(tries+1) else: - #print(f"Packet: {packet_signs.hex()}") base = self._format_sign(packet[9:11], packet_signs[9:11]) shoulder = self._format_sign(packet[11:13], packet_signs[11:13]) elbow = self._format_sign(packet[13:15], packet_signs[13:15]) @@ -86,8 +85,8 @@ class URModbusServer: def get_joint_speeds(self, tries=0): """ - Connects with the Modbus server to requests the speed of each joint, in radians - :return: Readable angle values of each joint in radials + Connects with the Modbus server to requests the speed of each joint, in radians per second + :return: Readable angle speeds of each joint in radians per second """ if tries>10: print("Modbus Error: Failed") @@ -96,11 +95,10 @@ class URModbusServer: packet = self.modbusTCP.read_holding_registers(280, quantity=6) if (packet is None): - time.sleep(0.01) + time.sleep(0.05) print(f"Modbus Error #{tries}: retrying") return self.get_joint_speeds(tries+1) else: - #print(f"Packet: {packet_signs.hex()}") base = self._format(packet[9:11]) / 1000 shoulder = self._format(packet[11:13]) / 1000 elbow = self._format(packet[13:15]) / 1000 diff --git a/URSentry.py b/URSentry.py index 864798c..9ad67ce 100644 --- a/URSentry.py +++ b/URSentry.py @@ -79,7 +79,7 @@ class URSentry: # print("Base Speed: ", self.robot_speed[0]) - ticks_to_wait = 10 + ticks_to_wait = 3 if self.await_stop: print("Awaiting stop --------------------") joint_speeds = self.robot.get_joint_speeds() @@ -95,6 +95,7 @@ class URSentry: self.await_stop = True self.send_to_zero_on_stop = False self.forward_position_to_base_angle_degrees(0) + return # Cancel smooth stopping due to no input if self.smooth_stop_delayed_call is not None: @@ -113,8 +114,8 @@ class URSentry: vertical_dead_zone_radius = 0.1 # Speeds for the base and neck joints - base_max_speed = 1.1 - base_min_speed = 0.2 + base_max_speed = 1.5 + base_min_speed = 0.1 base_acceleration = 1.5 @@ -156,7 +157,7 @@ class URSentry: ) self.smooth_stop_delayed_call.start() - print("Robot speed: ", self.robot_speed) + print("Base speed: ", self.robot_speed[0], " Joystick: ", joystick_pos_x) self.robot.speedj(self.robot_speed, base_acceleration, 1) diff --git a/main.py b/main.py index f5b4cdb..0107b15 100644 --- a/main.py +++ b/main.py @@ -25,8 +25,13 @@ class CameraJoystick(threading.Thread): def run(self): while self.keep_running: - bb = b.get_joystick_position_from_new_set_of_bboxes(q.get()) + 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) def stop(self): @@ -62,7 +67,7 @@ ur.forward_position() while True: try: ur.move_robot_base(-current_joystick[0]) - print("Setting robot base velocity to: ", current_joystick[0]) + #print("Setting robot base velocity to: ", current_joystick[0]) time.sleep(0.2) except KeyboardInterrupt: camera_joystick.stop()