def measure_triangle_tool(self, device: arduino_serial.Arduino,
world: plot_3d_sympy.DigitalWorldHandler,
translation: float,
acc=0.01,
vel=0.01): # translation in [m]
measure_points = (
(translation, -translation,),
(0, 2 * translation,),
(-2 * translation, -translation, 0)
)
self.cal_points = []
for point in measure_points:
# super().translate(point, acc, vel)
super().translate_tool( point, acc, vel )
dis = device.get_multiple_average( device.get_distance_top, 100 ) / 1000 # [m]
# dis -= 0.008 # correct for 8 mm of tcp offset of ultrasonic sensor
world.setl( super().getl() )
world.set_ultrasonic_top_distance( dis )
cur_pos = world.getl_ultrasonic_top()
self.cal_points.append( cur_pos )
# super().movel(start_pos, acc, vel)
world.world.set_plane( self.cal_points )
p1, p2, p3 = self.cal_points
self.plane.set_points( p1, p2, p3 )
self.plane.plane_calc()
def measure_triangle(self, device: arduino_serial.Arduino, translation: float, acc=0.01,
vel=0.01): # translation in [m]
measure_points = (
(translation, 0, translation),
(0, 0, -2 * translation),
(-2 * translation, 0.0, translation)
)
start_pos = super().getl()
tool_orient = [0, -2.22, -2.22]
start_pos[3] = tool_orient[0]
start_pos[4] = tool_orient[1]
start_pos[5] = tool_orient[2]
super().movel( start_pos, acc, vel )
for point in measure_points:
super().translate( point, acc, vel )
super().translate_tool()
dis = device.get_multiple_average( device.get_distance_top, 100 ) / 1000 # [m]
dis -= 0.008 # correct for 8 mm of tcp offset of ultrasonic sensor
# calculate the real position
cur_pos = super().getl()
# cur_pos[1] = cur_pos[1] - self.tcp[2]
cur_pos[1] = cur_pos[1] + dis
self.cal_points.append( cur_pos )
super().movel( start_pos, acc, vel )
p1, p2, p3 = self.cal_points
self.plane.set_points( p1, p2, p3 )
self.plane.plane_calc()
def centre_for_triangle(self, device: arduino_serial.Arduino, measurement_offset=-0.03, board_distance=-0.10):
# get current ditance from the side of the van
aruco_offset = device.get_multiple_average( device.get_distance_top, 100 ) / 1000 # [m]
aruco_offset -= 0.008 # correct for 8 mm of tcp offset of ultrasonic sensor
# set offsets for measurements
set_centre_measurement = aruco_offset + measurement_offset
set_triangle_safe_dis = aruco_offset + board_distance
# do measurements
self.translate( (0, set_centre_measurement, 0, 0, 0, 0), 0.1, 0.1 )
self.move_joint( 5, 45, 0.1, 0.5 )
time.sleep( 0.2 )
centre_check_1 = device.get_multiple_average( device.get_distance_side, 100 ) / 1000
self.move_joint( 5, 135, 0.1, 0.5 )
time.sleep( 0.2 )
centre_check_2 = device.get_multiple_average( device.get_distance_side, 100 ) / 1000
x_to_centre = 0.30 - centre_check_1
z_to_centre = centre_check_2 - 0.28
self.translate( (0, -(set_triangle_safe_dis + measurement_offset), 0, 0, 0, 0), 0.1, 0.3 )
self.translate( (x_to_centre, 0, z_to_centre, 0, 0, 0), 0.1, 0.1 )
def repetitive_def_axis(self, device: arduino_serial.Arduino, y_offset=-0.03, axis_measurement_offset=0.15):
'''
finds four points on the left and top side of the borders of the plane.
Points found, can be used to define x-axis (top) and z-axis (left).
:param device: arduino objects
:param axis_measurement_offset: safety distance from the borders of the plane
:param y_offset: perpendicular offset from plane
:return:
'''
points1 = []
points2 = []
# left 1
self.move_y_plane( y_offset )
self.move_joint( 5, 45, 0.1, 0.5 )
x_check = device.get_multiple_average( device.get_distance_side, 100 ) / 1000
cur_pos = self.getl()
cur_pos[0] -= (x_check - axis_measurement_offset)
self.move_y_plane( y_offset, cur_pos )
x_offset = device.get_multiple_average( device.get_distance_side, 100 ) / 1000
cur_pos = self.getl()
cur_pos[0] -= x_offset
points1.append( ['left', x_offset, cur_pos] )
# top 1
self.move_joint( 5, 135, 0.1, 0.5 )
z_check = device.get_multiple_average( device.get_distance_side, 100 ) / 1000
if z_check > 0.4:
z_check = 0.25
else:
z_check
z_start = self.getl()
safe_move = (z_check - axis_measurement_offset)
z_start[2] += (z_check - axis_measurement_offset)
self.move_y_plane( y_offset, z_start )
z_offset = device.get_multiple_average( device.get_distance_side, 100 ) / 1000
cur_pos = self.getl()
cur_pos[2] += z_offset
points1.append( ['top', z_offset, cur_pos] )
# print(points1)
# left 2
self.move_joint( 5, 45, 0.1, 0.5 )
x1_check = device.get_multiple_average( device.get_distance_side, 100 ) / 1000
cur_pos = self.getl()
cur_pos[0] -= x1_check
points2.append( ['left', x1_check, cur_pos] )
# top 2
last_point = self.getl()
last_point[0] += safe_move
self.move_y_plane( y_offset, last_point )
self.move_joint( 5, 135, 0.1, 0.5 )
z1_offset = device.get_multiple_average( device.get_distance_side, 100 ) / 1000
cur_pos = self.getl()
cur_pos[2] += z1_offset
points2.append( ['top', z1_offset, cur_pos] )
# print(points2)
return points1, points2
def measure_sides(self, device: arduino_serial.Arduino, side_list=None):
if side_list is None: # default value
side_list = ["left", "top"]
orientations = { # degrees, axis, pos/neg
"left": [45, "x", -1], # left
"top": [45 + 90 * 1, "z", 1], # top
"right": [45 + 90 * 2, "x", 1], # right
"bottom": [45 + 90 * 3, "z", -1] # bottom
}
# set orientation of tool right
start_pos = super().getl()
tool_orient = [0, -2.22, -2.22]
start_pos[3] = tool_orient[0]
start_pos[4] = tool_orient[1]
start_pos[5] = tool_orient[2]
super().movel( start_pos )
side_points = []
for side in side_list:
degrees, axis, direction = orientations[side]
self.move_joint( 5, degrees, 0.1, 0.5 )
distance = device.get_multiple_average( device.get_distance_side, 100 ) / 1000 # [m]
print( degrees, ' - ', distance )
# calc action point position
cur_pos = super().getl()
if axis == "x":
index = 0
elif axis == "y":
index = 1
elif axis == "z":
index = 2
else:
raise Exception( "Didn't find index" )
cur_pos[index] += distance * direction
side_points.append(
[side, distance, cur_pos] )
return side_points
def side_look(self, device: arduino_serial.Arduino):
# measure_list = [ # degrees, axis, pos/neg
# [45, "x", -1], # left
# [45 + 90 * 1, "z", 1], # top
# [45 + 90 * 2, "x", 1], # right
# [45 + 90 * 3, "z", -1] # bottom
# ]
measure_list = [ # degrees, axis, pos/neg
[45, "x", -1], # left
[45 + 90 * 1, "z", 1], # top
[45 + 90 * 2, "x", 1], # right
[45 + 90 * 3, "z", -1] # bottom
]
cur_pos = super().getl()
y_pos = self.plane.deviation_calc( cur_pos )
y_pos2 = y_pos - 0.010
cur_pos[1] = y_pos2
super().movel( cur_pos )
for degrees, axis, direction in measure_list:
self.move_joint( 5, degrees, 0.1, 0.5 )
distance = device.get_multiple_average( device.get_distance_side, 100 ) / 1000 # [m]
print( degrees, ' - ', distance )
# calc action point position
cur_pos = super().getl()
if axis == "x":
index = 0
elif axis == "y":
index = 1
elif axis == "z":
index = 2
else:
raise Exception( "Didn't find index" )
cur_pos[index] += distance * direction
self.side_points.append(
[degrees, distance, cur_pos] )
self.move_joint( 5, measure_list[0][0], 0.1, 0.5 )
from arduino_serial import Arduino
arduino = Arduino()
arduino.connect("COM4", 9600)
print("Turn LED On or Off.\n")
while arduino.isConnected():
switch = input("Turn light:")
if switch == "On":
arduino.setHigh()
elif switch == "Off":
arduino.setLow()
if switch == "q":
arduino.disconnect()
from arduino_serial import Arduino
arduino = Arduino()
arduino.connect("COM4", 9600)
while arduino.isConnected():
pos = input("Servo position: ")
arduino.sendCommand(pos)
if pos == "q":
arduino.disconnect()