UGA-Innovation-Factory
/
UR10-Sentry
mirror of https://github.com/UGA-Innovation-Factory/UR10-Sentry.git
2
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

V1.0.0 sentry follows properly

This commit is contained in:
Tomás L 2024-07-19 15:48:42 -04:00
parent 685c1db246
commit 9b8f4d60c0
5 changed files with 23 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
BBoxProcessor.py
View File

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

8
Communication/ModbusTCP.py
View File

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

10
Robot/UR/URModbusServer.py
View File

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

9
URSentry.py
View File

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

9
main.py
View File

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