def find_center_of_hole(video_capture):
s = linuxcnc.stat()
s.poll()
# moving = is_moving(s)
start_x = s.axis[0]['input']
start_y = s.axis[1]['input']
start_z = s.axis[2]['input']
left, _ = find_left_edge(video_capture)
_, _ = monitored_move_to(video_capture,
start_x,
start_y,
start_z,
feedrate=c_feedrate_fast)
right, _ = find_right_edge(video_capture)
dx = right[0] - left[0]
cmd_x = left[0] + dx / 2.
_, _ = monitored_move_to(video_capture,
cmd_x,
start_y,
start_z,
feedrate=c_feedrate_fast)
aft, _ = find_aft_edge(video_capture)
_, _ = monitored_move_to(video_capture,
cmd_x,
start_y,
start_z,
feedrate=c_feedrate_fast)
forward, _ = find_forward_edge(video_capture)
dy = forward[1] - aft[1]
cmd_y = aft[1] + dy / 2.
_, _ = monitored_move_to(video_capture,
cmd_x,
cmd_y,
start_z,
feedrate=c_feedrate_fast)
dz = 0
s.poll()
moving = is_moving(s)
print('moving:', moving)
x = s.axis[0]['input']
y = s.axis[1]['input']
z = s.axis[2]['input']
diam = dy
return (x, y, z), (x - start_x, y - start_y, z - start_z), (dx, dy,
dz), diam
def main():
cv2.namedWindow(c_camera_name)
cv2.setMouseCallback(c_camera_name, click_and_crop)
global cnc_s
global cnc_c
cnc_s = linuxcnc.stat()
cnc_c = linuxcnc.command()
video_capture = haimer_camera.gauge_vision_setup()
global video_capture2
video_capture2 = z_camera.gauge_vision_setup()
while True:
try:
_ = update_view(video_capture, video_capture2)
except OvershootException as e:
cnc_c.abort()
common.display_error(str(e))
except NotReady as e:
common.display_error(str(e))
except common.InvalidImage:
s = linuxcnc.stat()
s.poll()
moving = is_moving(s)
cnc_c.abort()
print('Invalid image', moving)
sys.exit(1)
except common.QuitException:
s = linuxcnc.stat()
s.poll()
moving = is_moving(s)
cnc_c.abort()
if moving:
print('Quit requested while still moving')
sys.exit(1)
else:
break
def find_edge(video_capture, direction):
cnc_c.mode(linuxcnc.MODE_MDI)
cnc_c.wait_complete()
state = 0
d1 = None
in_state_3 = 0
start_x = None
start_y = None
start_z = None
tolerance_e = .0005
while True:
s = linuxcnc.stat()
s.poll()
print(s.axis[0]['input'], s.axis[0]['output'], s.axis[0]['homed'],
s.axis[0]['velocity'], s.axis[0]['enabled'])
moving = is_moving(s)
x = s.axis[0]['input']
y = s.axis[1]['input']
z = s.axis[2]['input']
print(s.axis[0])
mm_final, circles = update_view(video_capture, video_capture2)
if mm_final is None:
print('mm_final is None')
continue
if mm_final > 1.0:
cnc_c.abort()
raise OvershootException(mm_final)
in_final = mm_final / 25.4
xe = in_final * direction[0]
ye = in_final * direction[1]
ze = in_final * direction[2]
# print('mm_final:', mm_final, 'in_final:', in_final, 'cur_x:', x, 'tar_x:', tar_x, 'xe:', xe, 'cmd_x:', cmd_x, ok_for_mdi(s))
div_f = 2
if not ok_for_mdi(s) and not moving:
if not wait_for_ready(s):
raise NotReady()
elif ok_for_mdi(s) and not moving:
# print('state', state, 'total_e:', total_e, 'in_final:', in_final, 'mm_final:', mm_final)
print('state', state, 'in_final:', in_final, 'mm_final:', mm_final)
total_e = 0.
if state == 0:
start_x = x
start_y = y
start_z = z
state = 1
continue
elif state == 1:
cmd_x = x - xe
cmd_y = y - ye
cmd_z = z - ze
move_to(cmd_x, cmd_y, cmd_z)
elif state == 2:
if d1 is None:
d1 = time.time()
else:
dt = time.time() - d1
dwell_time = c_fast_dwell if 1.5 < abs(
mm_final) else c_slow_dwell
if dt < dwell_time:
print('In settling period', dt, '<', dwell_time)
else:
d1 = None
total_e = abs(xe) + abs(ye) + abs(ze)
if dwell_time == c_fast_dwell:
state = 1
elif total_e > tolerance_e:
print('total_e', total_e)
state = 3
else:
state = 4
continue
elif state == 3:
in_state_3 += 1
if in_state_3 > 10:
# In case the oscillations do not stop, exit the loop. The error will be tiny.
print(
'Aborting due to excessive oscillation, current total_e = {} > {}'
.format(total_e, tolerance_e))
state = 4
continue
elif in_state_3 > 5:
# Could alternatively increase the finishing error tolerance
# Best would be to back away from and reapproach the edge
print(
'Oscillation may be occuring, current total_e = {} > {}, reducing div_f'
.format(total_e, tolerance_e))
div_f = 4
if abs(mm_final) > .05:
cmd_x = x - xe * .95
cmd_y = y - ye * .95
cmd_z = z - ze * .95
else:
print('part2')
cmd_x = x - xe / div_f
cmd_y = y - ye / div_f
cmd_z = z - ze / div_f
move_to(cmd_x, cmd_y, cmd_z)
state = 2
elif state == 4:
print('Done, edge found at:', x, y, z, 'with error', total_e)
break
else:
print('Waiting for move to complete.')
if state == 1:
print('mm_final', mm_final)
if abs(mm_final) < 1.5:
print('mm_final0', mm_final)
cnc_c.abort()
state = 2
continue
sys.stdout.flush()
return (x, y, z), (x - start_x, y - start_y, z - start_z)
def find_corner(video_capture, direction):
s = linuxcnc.stat()
s.poll()
# moving = is_moving(s)
start_x = s.axis[0]['input']
start_y = s.axis[1]['input']
start_z = s.axis[2]['input']
ball_diam = 4. / 25.4
if direction == 'ul':
edge_x, _ = find_left_edge(video_capture)
_, _ = monitored_move_to(video_capture, edge_x[0] - ball_diam, start_y,
start_z)
_, _ = monitored_move_to(video_capture, edge_x[0] - ball_diam,
start_y + 0.5, start_z)
_, _ = monitored_move_to(video_capture, edge_x[0] + ball_diam,
start_y + 0.5, start_z)
edge_y, _ = find_aft_edge(video_capture)
elif direction == 'ur':
edge_x, _ = find_right_edge(video_capture)
_, _ = monitored_move_to(video_capture, edge_x[0] + ball_diam, start_y,
start_z)
_, _ = monitored_move_to(video_capture, edge_x[0] + ball_diam,
start_y + 0.5, start_z)
_, _ = monitored_move_to(video_capture, edge_x[0] - ball_diam,
start_y + 0.5, start_z)
edge_y, _ = find_aft_edge(video_capture)
elif direction == 'll':
edge_x, _ = find_left_edge(video_capture)
_, _ = monitored_move_to(video_capture, edge_x[0] - ball_diam, start_y,
start_z)
_, _ = monitored_move_to(video_capture, edge_x[0] - ball_diam,
start_y - 0.5, start_z)
_, _ = monitored_move_to(video_capture, edge_x[0] + ball_diam,
start_y - 0.5, start_z)
edge_y, _ = find_forward_edge(video_capture)
elif direction == 'lr':
edge_x, _ = find_right_edge(video_capture)
_, _ = monitored_move_to(video_capture, edge_x[0] + ball_diam, start_y,
start_z)
_, _ = monitored_move_to(video_capture, edge_x[0] + ball_diam,
start_y - 0.5, start_z)
_, _ = monitored_move_to(video_capture, edge_x[0] - ball_diam,
start_y - 0.5, start_z)
edge_y, _ = find_forward_edge(video_capture)
else:
edge_x = []
edge_y = []
s.poll()
moving = is_moving(s)
print('moving:', moving)
x = edge_x[0]
y = edge_y[1]
z = s.axis[2]['input']
return (x, y, z), (x - start_x, y - start_y, z - start_z)
def monitored_move_to(video_capture,
cmd_x,
cmd_y,
cmd_z,
max_mm=1.0,
local=False,
feedrate=None):
if feedrate is None:
feedrate = c_feedrate
cnc_c.mode(linuxcnc.MODE_MDI)
cnc_c.wait_complete()
if local:
print('Converting:', [cmd_x, cmd_y, cmd_z])
cmd_x, cmd_y, cmd_z = part_to_machine_cs([cmd_x, cmd_y, cmd_z])
print('Converted:', [cmd_x, cmd_y, cmd_z])
state = 0
start_x = None
start_y = None
start_z = None
while True:
s = linuxcnc.stat()
s.poll()
print(s.axis[0]['input'], s.axis[0]['output'], s.axis[0]['homed'],
s.axis[0]['velocity'], s.axis[0]['enabled'])
moving = is_moving(s)
x = s.axis[0]['input']
y = s.axis[1]['input']
z = s.axis[2]['input']
mm_final, circles = update_view(video_capture, video_capture2)
if mm_final is None:
print('mm_final is None')
continue
if mm_final > max_mm:
cnc_c.abort()
raise OvershootException(mm_final)
if not ok_for_mdi(s) and not moving:
if not wait_for_ready(s):
raise NotReady()
elif ok_for_mdi(s) and not moving and state != 2:
print('state', state, 'mm_final:', mm_final)
if state == 0:
start_x = x
start_y = y
start_z = z
state = 1
continue
elif state == 1:
move_to(cmd_x, cmd_y, cmd_z, feedrate=feedrate)
# TODO: Reconsider this logic
# State == 2 was added because if the commanded position is achieved before 'ok_for_mdi(s) and not moving'
# is considered again, there can be a loop created in state == 1. To get to step 3, the original step 2,
# there must be a period where the machine is seen to be moving. The most extreme is when start = cmd, but
# it's not the only case, because some time is needed for the video to update and the move could have
# completed by the next time through.
state = 2
elif state == 3:
dx = x - cmd_x
dy = y - cmd_y
dz = z - cmd_z
total_e = abs(dx) + abs(dy) + abs(dz)
print('Done, move to', x, y, z, 'completed with error',
total_e)
break
else:
print('Waiting for move to complete.')
if state == 2:
print('mm_final', mm_final)
if ok_for_mdi(s) and not moving:
state = 3
sys.stdout.flush()
return (x, y, z), (x - start_x, y - start_y, z - start_z)