def linear_move(self, x_e, y_e, pv_x, pv_y, dv_x, dv_y, min_d):
# calculates end position of beta arm movement and direction of movement
d = sqrt(x_e**2 + y_e**2) # distance to center at end position
beta_e = 2 * asin(d / (2 * self.rB)) # value of beta at end position
dir_beta = sign(beta_e - self.beta) # direction of beta movement
if abs(beta_e - self.beta) <= 0.5 * self.BETA_RES:
# if true, beta is already as close to it's target position as possible
# although we don't need to move the beta arm, we might still need to move the table (alpha)
path = list()
steps_alpha, dir_alpha = self.calc_alpha(x_e, y_e)
self.x = x_e
self.y = y_e
if steps_alpha != 0:
# alpha needs to move
self.segment_index += 1
path.append(
PathSegment(dir_alpha * steps_alpha, 0, 0, self.x, self.y,
self.z, self.alpha, self.beta,
self.current_gcommand, self.segment_index))
return path
else:
# beta needs to move
return self.linear_move_beta(beta_e, dir_beta, min_d, x_e, y_e,
pv_x, pv_y, dv_x, dv_y)
def arc_move_beta(self, x_e, y_e, m, n, r, min_d, pos_neg):
path = []
if self.x == x_e and self.y == y_e:
# no movement necessary
return path
# calculate end position of beta arm movement (and direction)
d = sqrt(x_e**2 + y_e**2)
beta_e = 2 * asin(d / (2 * self.rB))
dir_beta = sign(beta_e - self.beta)
dir_x = sign(x_e - self.x)
last_iteration = False
while True:
self.beta = self.beta + (
self.BETA_RES * dir_beta
) # move beta arm one res step per iteration
# check whether this is the last iteration of the loop
if (dir_beta == 1
and self.beta > beta_e) or (dir_beta == -1
and self.beta < beta_e):
last_iteration = True
d_c = 2 * self.rB * sin(
self.beta / 2) # current distance of beta arm from centre
if not last_iteration:
if d_c < min_d: # calc_xy_arc fails if this is true
d_c = min_d # may be caused by very slight calculation errors
# get current x/y position
self.x, self.y = self.calc_xy_arc(m, n, r, d_c, pos_neg, dir_x)
else:
# compensate for rounding errors on the last go by simply using the target values
# as current position; seems to be fine
self.x = x_e
self.y = y_e
if self.DEBUGOUT: # dev stuff
self.DEBUGOUT.update_position(self.x, self.y, 'arc')
steps_alpha, dir_alpha = self.calc_alpha(self.x, self.y)
# add to path
self.segment_index += 1
path_segment = PathSegment(dir_alpha * steps_alpha, dir_beta, 0,
self.x, self.y, self.z, self.alpha,
self.beta, self.current_gcommand,
self.segment_index)
path.append(path_segment)
# checking whether to continue loop or not
if last_iteration:
return path
def move_z(self, z):
# calculate number of steps to move
z_dif = z - self.z
z_steps = int(round(z_dif / self.Z_RES, 0))
# set current z position
self.z += z_steps * self.Z_RES
# create and return path segment
self.segment_index += 1
return [
PathSegment(0, 0, z_steps, self.x, self.y, self.z, self.alpha,
self.beta, self.current_gcommand, self.segment_index)
]
def recalculate_segment_buffer(self):
# reverse pass through the processing buffer
# always starts with an empty segment at the end to represent a stop
current_seg = PathSegment(0, 0, 0, 0, 0, 0, 0, 0, None, None)
for idx in range(-1,
-len(self.processing_buffer) + self.buffer_planned,
-1):
next_seg = current_seg
current_seg = self.processing_buffer[idx]
# calculate the maximum entry speed by decelerating over the current segment
# (mathematically we accelerate in reverse)
entry_speed_sqr = next_seg.entry_speed_sqr + 2 * current_seg.acceleration * current_seg.distance
if entry_speed_sqr < current_seg.max_entry_speed_sqr:
current_seg.entry_speed_sqr = entry_speed_sqr
else:
current_seg.entry_speed_sqr = current_seg.max_entry_speed_sqr
# forward_pass
next_seg = self.processing_buffer[self.buffer_planned]
for idx in range(self.buffer_planned + 1, len(self.processing_buffer),
1):
current_seg = next_seg
next_seg = self.processing_buffer[idx]
# calculate maximum entry speed by accelerating over the current segment
if current_seg.entry_speed_sqr < next_seg.entry_speed_sqr:
entry_speed_sqr = current_seg.entry_speed_sqr + 2 * current_seg.acceleration * current_seg.distance
if entry_speed_sqr < next_seg.entry_speed_sqr:
next_seg.entry_speed_sqr = entry_speed_sqr
# the current segment is at maximum acceleration, the buffer can not be improved anymore up to here
# the buffer_planned indicator gets set to the new position
self.buffer_planned = idx - 2
def linear_move_beta(self, beta_e, dir_beta, min_d, x_e, y_e, pv_x, pv_y,
dv_x, dv_y):
# beta is moved in steps of it minimum resolution
# x, y and alpha are calculated for every iteration
path = list()
last_iteration = False
while True:
# move beta arm one res step per iteration
self.beta = self.beta + (self.BETA_RES * dir_beta)
# check whether this is the last iteration of the loop
if abs(beta_e - self.beta) <= 0.5 * self.BETA_RES:
last_iteration = True
# current distance of beta arm from center
d_c = 2 * self.rB * sin(self.beta / 2)
if not last_iteration:
if d_c < min_d: # calc_xy_linear fails if this is true
d_c = min_d # may be caused by very slight calculation errors
# get current x/y position
self.x, self.y = self.calc_xy_linear(x_e, y_e, pv_x, pv_y,
dv_x, dv_y, d_c)
else:
# compensate for small calculation errors on the last go by simply using the target values
# as current position; seems to be fine
self.x = x_e
self.y = y_e
if self.DEBUGOUT: # dev stuff
self.DEBUGOUT.update_position(self.x, self.y, 'linear')
# calculate alpha position
steps_alpha, dir_alpha = self.calc_alpha(self.x, self.y)
# add to path
self.segment_index += 1
path_segment = PathSegment(dir_alpha * steps_alpha, dir_beta, 0,
self.x, self.y, self.z, self.alpha,
self.beta, self.current_gcommand,
self.segment_index)
path.append(path_segment)
# checking whether to continue loop or not
if last_iteration:
return path
def serve_next(self):
while 'this loop just goes on until a segment is returned':
if self.bypass and self.bypass[
0].index == self.last_segment_index + 1:
self.last_segment_index = self.bypass[0].index
return self.bypass.pop(0)
while not self.next:
# should only happen on first call and maybe next few
# while there has not been a usable segment for self.next yet
next_segment = self.provider.serve_next()
if next_segment.a_steps and next_segment.b_steps:
self.next = next_segment
else:
self.last_segment_index = next_segment.index
return next_segment
segment = self.provider.serve_next()
if not segment:
segment = PathSegment(0, 0, 0, 0, 0, 0, 0, 0, None, None)
elif not segment.a_steps and not segment.b_steps:
self.bypass.append(segment)
continue
self.current = self.next
self.next = segment
current_max_rate_a, current_max_rate_b = self.nominal_rate(
self.current)
next_max_entry_rate_a, next_max_entry_rate_b = self.max_entry_rate(
self.next)
self.accelerate_a(self.current, current_max_rate_a,
next_max_entry_rate_a)
self.accelerate_b(self.current, current_max_rate_b,
next_max_entry_rate_b)
self.last_segment_index = self.current.index
return self.current
def arc_process(self, x_e, y_e, i, j, mv_dir):
# prepares for circular interpolation and splits circle into segments
# Currently circular interpolation is limited to circle segments covering
# 180 degrees of a full circle at maximum. Larger segments need to be split
# up into multiple G02/G03 commands prior to handing them to this class!
#
# This code works with the equation for a half circle:
# f(x) = sqrt(r^2 + (x-m)^2) + n
#
# A full circle would need to be split into four segments. First into two segments
# as the equation used only defines the upper OR lower half of a circle (+f(x) or -f(x)).
# Secondly these segments are both split again at the points closest to and furthest away
# from the table's center, as the beta arm needs to change direction at these points.
# Although full circles are not supported currently, these split points also apply to
# segments of a circle.
path = list()
m = self.x + i # center coordinates of the circle
n = self.y + j
r = sqrt((m - self.x)**2 + (n - self.y)**2) # radius
# if the circle's center point is the same as the table's center point, only alpha needs to move.
# returns path empty if alpha also does not need to move
if m == 0 and n == 0:
steps_alpha, dir_alpha = self.calc_alpha(x_e, y_e)
self.x = x_e
self.y = y_e
if steps_alpha != 0:
self.segment_index += 1
path.append(
PathSegment(dir_alpha * steps_alpha, 0, 0, self.x, self.y,
self.z, self.alpha, self.beta,
self.current_gcommand, self.segment_index))
return path
# the beta axis needs to move
else:
# Calculate points closest to and furthest away from table center and corresponding min/max dist.
# Depending on the position on the table each of these calculation can either return the closest
# or the furthes point, therefore further checks are necessary.
p1_x = m + m * (1 / sqrt(m**2 + n**2)) * r
p1_y = n + n * (1 / sqrt(m**2 + n**2)) * r
p2_x = m - m * (1 / sqrt(m**2 + n**2)) * r
p2_y = n - n * (1 / sqrt(m**2 + n**2)) * r
d_1 = sqrt(p1_x**2 + p1_y**2)
d_2 = sqrt(p2_x**2 + p2_y**2)
# Determine which is maximum and minimum distance from table center.
min_d = min(d_1, d_2)
# Determine upper and lower point at which the beta arm needs to change direction.
# The upper point is on the positive circle half, the lower point on the negative circle half
if not p1_y == p2_y:
if p1_y > p2_y:
p_up_y = p1_y
p_up_x = p1_x
p_low_y = p2_y
p_low_x = p2_x
else:
p_up_y = p2_y
p_up_x = p2_x
p_low_y = p1_y
p_low_x = p1_x
else:
# which one is which is irrelevant
p_up_y = p1_y
p_up_x = p1_x
p_low_y = p2_y
p_low_x = p2_x
# The circle is devided into four segments, c_segs contains the split points:
# (m +-r, n) are necessary as f(x) only defines a half circle; they split top and bottom half
# (p*_x, p*_y) are necessary as the beta arm needs to change direction at these points
# The code checks where the start and end points fit in and then iterates from start over the split points
# until it reaches the end point. Per iteration, interpolation from the current position to the next point
# is performed.
# Taking the list times three ensures that we don't need to loop over to the beginning while iterating.
# The list order is changed for depending on direction of movement.
if mv_dir == 'G02':
c_segs = [(m - r, n), (p_up_x, p_up_y), (m + r, n),
(p_low_x, p_low_y)] * 3
else:
c_segs = [(m + r, n), (p_up_x, p_up_y), (m - r, n),
(p_low_x, p_low_y)] * 3
# find position of start and endpoint in c_segs
#
# self.x/self.y are the starting point's coordinates
if not m - r <= self.x <= m + r:
# because of slight inaccuracies it can happen that self.x is not on/in the circle
# in that case it is just set to the min/max possible value
if abs(self.x - (m - r)) < abs(x_e - (m + r)):
self.x = m - r
else:
self.x = m + r
for i in range(7): # starting point
# check if current x is between the current and the next split point
if c_segs[i][0] >= self.x >= c_segs[
i + 1][0] or c_segs[i][0] <= self.x <= c_segs[i +
1][0]:
if i in (0, 1, 4,
5): # +f(x) / segment is upper circle half
# n is the circle centre's y-coordinate; the current segment is on the upper
# half of the circle, therefore self.y also needs to be above the circle's centre
# else the current point does not fit in at this position; continue searching...
if self.y >= n:
i_start = i + 1
break
elif i in (2, 3, 6,
7): # -f(x) / segment is lower circle half
if self.y < n:
i_start = i + 1
break
if not m - r <= x_e <= m + r:
# because of slight inaccuracies it can happen that x_e is not on/in the circle
# in that case it is just set to the min/max possible value
if abs(x_e - (m - r)) < abs(x_e - (m + r)):
x_e = m - r
else:
x_e = m + r
# look were the endpoint fits in; basically same as for starting point above
for i in range(i_start - 1, 11): # endpoint
if c_segs[i][0] >= x_e >= c_segs[
i + 1][0] or c_segs[i][0] <= x_e <= c_segs[i + 1][0]:
if i in (0, 1, 4, 5, 8, 9): # +f(x)
if y_e >= n:
i_end = i + 1
break
elif i in (2, 3, 6, 7, 10, 11): # -f(x)
if y_e < n:
i_end = i + 1
break
# pos_neg indicates whether a segment is on the upper (pos) or lower (neg) half of the circle.
# this value is passed on to be used later for calculating the current x and y coordinates
pos_neg = [-1, 1, 1, -1] * 3
# do interpolation for each circle segment
for i in range(i_start, i_end):
path += self.arc_move_beta(c_segs[i][0], c_segs[i][1], m, n, r,
min_d, pos_neg[i])
# do interpolation to end position
path += self.arc_move_beta(x_e, y_e, m, n, r, min_d,
pos_neg[i + 1])
return path