ManuelW/grbl-LPC-CoreXY
1
0
Fork 0
Code Issues Pull Requests Releases 1 Wiki Activity

arc code complete with support for both R and IJK style blocks

Browse Source
This commit is contained in:
Simen Svale Skogsrud 2009-02-01 11:58:21 +01:00
parent e21064bd86
commit 9799955555
6 changed files with 161 additions and 213 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

174
arc_algorithm/arc.rb
View File

@ -244,160 +244,6 @@ class CircleTest
return {:tx => tx, :ty => ty, :x => x, :y => y}
end
# A DDA-direct search circle interpolator unrolled for each octant. Optimal and impure
def arc_unrolled(theta, angular_travel, radius)
radius = radius
x = (sin(theta)*radius).round
y = (cos(theta)*radius).round
angular_direction = angular_travel.sign
tx = (sin(theta+angular_travel)*(radius-0.5)).floor
ty = (cos(theta+angular_travel)*(radius-0.5)).floor
f = (x**2 + y**2 - radius**2).round
x2 = 2*x
y2 = 2*y
dx = (y==0) ? -x.sign : y.sign*angular_direction
dy = (x==0) ? -y.sign : -x.sign*angular_direction
max_steps = (angular_travel.abs*radius*2).floor
# Quandrants of the circls
# \ 1|2 /
# 8\ | / 3
# \|/
# ---------|-----------
# 7 /|\ 4
# / | \
# / 6 | 5 \
#
#
#
if angular_direction>0 # clockwise
if x.abs<y.abs # quad 1,2,6,5
if y>0 # quad 1,2
while x<0 # quad 1 x+,y+
x += 1
f += 1+x2
x2 += 2
f_diagonal = f + 1 + y2
if (f.abs >= f_diagonal.abs)
y += 1
y2 += 2
f = f_diagonal
end
end
while x>=0 # quad 2, x+, y-
x += 1
f += 1+x2
x2 += 2
f_diagonal = f + 1 - y2
if (f.abs >= f_diagonal.abs)
y -= 1
y2 -= 2
f = f_diagonal
end
end
end
if y<=0 # quad 6, 5
while x<0 # quad 6 x-, y+
x -= 1
f += 1-x2
x2 -= 2
f_diagonal = f + 1 + y2
if (f.abs >= f_diagonal.abs)
y += 1
y2 += 2
f = f_diagonal
end
end
while x>=0 # quad 5 x-, y-
x -= 1
f += 1-x2
x2 -= 2
f_diagonal = f + 1 - y2
if (f.abs >= f_diagonal.abs)
y -= 1
y2 -= 2
f = f_diagonal
end
end
end
# Quandrants of the circls
# \ 1|2 /
# 8\ | / 3
# \|/
# ---------|-----------
# 7 /|\ 4
# / | \
# / 6 | 5 \
else 3 # quad 3,4,7,8
if x>0 # quad 3,4
while y>0 # quad 3 x+,y+
x += 1
f += 1+x2
x2 += 2
f_diagonal = f + 1 + y2
if (f.abs >= f_diagonal.abs)
y += 1
y2 += 2
f = f_diagonal
end
end
while x>=0 # quad 2, x+, y-
x += 1
f += 1+x2
x2 += 2
f_diagonal = f + 1 - y2
if (f.abs >= f_diagonal.abs)
y -= 1
y2 -= 2
f = f_diagonal
end
end
end
if y<=0 # quad 6, 5
while x<0 # quad 6 x-, y+
x -= 1
f += 1-x2
x2 -= 2
f_diagonal = f + 1 + y2
if (f.abs >= f_diagonal.abs)
y += 1
y2 += 2
f = f_diagonal
end
end
while x>=0 # quad 5 x-, y-
x -= 1
f += 1-x2
x2 -= 2
f_diagonal = f + 1 - y2
if (f.abs >= f_diagonal.abs)
y -= 1
y2 -= 2
f = f_diagonal
end
end
end
else
y += dy
f += 1+y2*dy
y2 += 2*dy
f_diagonal = f + 1 + x2*dx
if (f.abs >= f_diagonal.abs)
x += dx
dy = -x.sign*angular_direction unless x == 0
x2 += 2*dx
f = f_diagonal
end
dx = y.sign*angular_direction unless y == 0
end
break if x*ty.sign*angular_direction>=tx*ty.sign*angular_direction && y*tx.sign*angular_direction<=ty*tx.sign*angular_direction
end
plot_pixel(tx+20, -ty+20, "o")
return {:tx => tx, :ty => ty, :x => x, :y => y}
end
end
@ -405,16 +251,16 @@ end
test = CircleTest.new
test.init
#test.arc_clean(0, Math::PI*2, 5)
(1..10000).each do |r|
test.init
data = test.arc_supaoptimal(2.9, Math::PI*1, r)
if (data[:tx]-data[:x]).abs > 1 || (data[:ty]-data[:y]).abs > 1
puts "r=#{r} fails target control"
pp data
puts
end
end
test.arc_clean(0, Math::PI*2, 3)
# (1..10000).each do |r|
# test.init
# data = test.arc_supaoptimal(2.9, Math::PI*1, r)
# if (data[:tx]-data[:x]).abs > 1 || (data[:ty]-data[:y]).abs > 1
# puts "r=#{r} fails target control"
# pp data
# puts
# end
# end
# test.init
# data = test.arc_supaoptimal(1.1, -Math::PI, 19)

3
config.h
View File

@ -68,4 +68,7 @@
#define BAUD_RATE 14400
// Unrolling the arc code is faster, but costs about 830 bytes of extra code space.
// #define UNROLLED_ARC_LOOP
#endif

111
gcode.c
View File

@ -241,25 +241,100 @@ uint8_t gc_execute_line(char *line) {
break;
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
if (radius_mode) {
// To be implemented
} else { // ijk-mode
// calculate the theta (angle) of the current point
double theta_start = theta(-offset[state.plane_axis_0], -offset[state.plane_axis_1]);
// calculate the theta (angle) of the target point
double theta_end = theta(target[state.plane_axis_0] - offset[state.plane_axis_0] - state.position[state.plane_axis_0],
target[state.plane_axis_1] - offset[state.plane_axis_1] - state.position[state.plane_axis_1]);
// ensure that the difference is positive so that we have clockwise travel
if (theta_end < theta_start) { theta_end += 2*M_PI; }
double angular_travel = fabs(theta_end-theta_start);
// Invert angular motion if we want a counter clockwise arc
if (next_action == MOTION_MODE_CCW_ARC) {
angular_travel = angular_travel-2*M_PI;
}
// Find the radius
double radius = hypot(offset[state.plane_axis_0], offset[state.plane_axis_1]);
// Prepare the arc
mc_arc(theta_start, angular_travel, radius, state.plane_axis_0, state.plane_axis_1, state.feed_rate);
/*
We need to calculate the center of the circle that has the designated radius and passes
through both the current position and the target position. This method calculates the following
set of equations where [x,y] is the vector from current to target position, d == distance of
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
the center of the travel vector. This is the distance from the center of the travel vector
to the center of our circle. A perpendicular to the travel vector is scaled to the length of
h and added to the center of the travel vector to form the new point [i,j] which will be
the center of our circle.
d^2 == x^2 + y^2
h^2 == r^2 + (d/2)^2
i == x/2 - y/d*h
j == y/2 + x/d*h
O <- [i,j]
- |
r - |
- |
- | h
- |
[0,0] -> C -----------------+--------------- T <- [x,y]
| <------ d/2 ---->|
C - Current position
T - Target position
O - center of circle that pass through both C and T
d - distance from C to T
r - designated radius
h - distance from center of CT to O
Expanding the equations:
h = sqrt(4 r^2 + x^2 + y^2)/2
d = sqrt(x^2 + y^2)
i = x/2 - (h * y)/d
j = y/2 + (h * x)/d
Which can be written:
i = (x - (y * sqrt(4 * r^2 + x^2 + y^2))/sqrt(x^2 + y^2))/2
j = (y + (x * sqrt(4 * r^2 + x^2 + y^2))/sqrt(x^2 + y^2))/2
Which can be optimized to:
h_x2_div_d = sqrt(4 * r^2 + x^2 + y^2)/sqrt(x^2 + y^2)
i = (x - (y * h_x2_div_d))/2
j = (y + (x * h_x2_div_d))/2
*/
// Calculate the change in position along each selected axis
double x = target[state.plane_axis_0]-state.position[state.plane_axis_0];
double y = target[state.plane_axis_1]-state.position[state.plane_axis_1];
clear_vector(&offset);
double h_x2_div_d = sqrt(4*r*r + x*x + y*y)/hypot(x,y); // == h * 2 / d
// The anti-clockwise circle lies to the right of the target direction. When offset is positive,
// the left hand circle will be generated - when it is negative the right hand circle is generated.
if (state.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
offset[state.plane_axis_0] = (x-(y*h_x2_div_d))/2;
offset[state.plane_axis_1] = (y+(x*h_x2_div_d))/2;
}
/*
This segment sets up an clockwise or counterclockwise arc from the current position to the target position around
the center designated by the offset vector. All theta-values measured in radians of deviance from the positive
y-axis.
| <- theta == 0
* * *
* *
* *
* O ----T <- theta == theta_end (e.g. 90 degrees: theta == PI/2)
* /
C <- theta == theta_start (e.g. -145 degrees: theta == -PI*(3/4))
*/
// calculate the theta (angle) of the current point
double theta_start = theta(-offset[state.plane_axis_0], -offset[state.plane_axis_1]);
// calculate the theta (angle) of the target point
double theta_end = theta(target[state.plane_axis_0] - offset[state.plane_axis_0] - state.position[state.plane_axis_0],
target[state.plane_axis_1] - offset[state.plane_axis_1] - state.position[state.plane_axis_1]);
// ensure that the difference is positive so that we have clockwise travel
if (theta_end < theta_start) { theta_end += 2*M_PI; }
double angular_travel = theta_end-theta_start;
// Invert angular motion if the g-code wanted a counterclockwise arc
if (state.motion_mode == MOTION_MODE_CCW_ARC) {
angular_travel = angular_travel-2*M_PI;
}
// Find the radius
double radius = hypot(offset[state.plane_axis_0], offset[state.plane_axis_1]);
// Prepare the arc
mc_arc(theta_start, angular_travel, radius, state.plane_axis_0, state.plane_axis_1, state.feed_rate);
break;
}
}

4
geometry.c
View File

@ -36,7 +36,3 @@ double theta(double x, double y)
}
}
double hypot(double x, double y)
{
sqrt(x*x + y*y);
}

3
geometry.h
View File

@ -24,7 +24,4 @@
// Find the angle from the positive y axis to the given point with respect to origo.
double theta(double x, double y);
// Find the distance from origo to point [x,y]
double hypot(double x, double y);
#endif

79
motion_control.c
View File

@ -58,6 +58,7 @@ struct ArcMotionParameters {
uint8_t axis_x, axis_y; // maps the arc axes to stepper axes
int32_t target[3]; // The target position in absolute steps
int8_t plane_steppers[3]; // A vector with the steppers of axis_x and axis_y set to 1, the remaining 0
int incomplete; // True if the arc has not reached its target yet
};
/* The whole state of the motion-control-system in one struct. Makes the code a little bit hard to
@ -81,6 +82,7 @@ uint8_t direction_bits; // The direction bits to be used with any upcoming step-
void set_stepper_directions(int8_t *direction);
inline void step_steppers(uint8_t *enabled);
inline void step_axis(uint8_t axis);
void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate);
void mc_init()
{
@ -98,11 +100,13 @@ void mc_dwell(uint32_t milliseconds)
// unless invert_feed_rate is true. Then the feed_rate states the number of seconds for the whole movement.
void mc_linear_motion(double x, double y, double z, float feed_rate, int invert_feed_rate)
{
state.mode = MC_MODE_LINEAR;
state.linear.target[X_AXIS] = trunc(x*X_STEPS_PER_MM);
state.linear.target[Y_AXIS] = trunc(y*Y_STEPS_PER_MM);
state.linear.target[Z_AXIS] = trunc(z*Z_STEPS_PER_MM);
prepare_linear_motion(trunc(x*X_STEPS_PER_MM), trunc(y*Y_STEPS_PER_MM), trunc(z*Z_STEPS_PER_MM), feed_rate, invert_feed_rate);
}
// Same as mc_linear_motion but accepts target in absolute step coordinates
void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate)
{
state.mode = MC_MODE_LINEAR;
uint8_t axis; // loop variable
// Determine direction and travel magnitude for each axis
@ -202,6 +206,7 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
// mm/second -> microseconds/step. Assumes all axes have the same steps/mm as the x axis
state.pace =
ONE_MINUTE_OF_MICROSECONDS / (feed_rate * X_STEPS_PER_MM);
state.arc.incomplete = true;
}
#define check_arc_target \
@ -209,7 +214,7 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
state.arc.target_x * state.arc.target_direction_y) && \
(state.arc.y * state.arc.target_direction_x <= \
state.arc.target_y * state.arc.target_direction_x)) \
{ state.mode = MC_MODE_AT_REST; }
{ state.arc.incomplete = false; }
// Internal method used by execute_arc to trace horizontally in the general direction provided by dx and dy
void step_arc_along_x(int8_t dx, int8_t dy)
@ -259,6 +264,8 @@ void map_local_arc_directions_to_stepper_directions(int8_t dx, int8_t dy)
set_stepper_directions(direction);
}
/*
Quandrants of the arc
\ 7|0 /
@ -269,6 +276,7 @@ void map_local_arc_directions_to_stepper_directions(int8_t dx, int8_t dy)
/ | \
x- / 4|3 \ x+ */
#ifdef UNROLLED_ARC_LOOP // This function only used by the unrolled arc loop
// Determine within which quadrant of the circle the provided coordinate falls
int quadrant(uint32_t x,uint32_t y)
{
@ -289,70 +297,93 @@ int quadrant(uint32_t x,uint32_t y)
}
}
}
#endif
// Will trace the configured arc until the target is reached. Slightly unrolled for speed.
// Will trace the configured arc until the target is reached.
void execute_arc()
{
int q = quadrant(state.arc.x, state.arc.y);
uint32_t start_x = state.arc.x;
uint32_t start_y = state.arc.y;
int dx, dy; // Trace directions
// state.mode is set to 0 (MC_MODE_AT_REST) when target is reached
while(state.mode == MC_MODE_ARC)
while(state.arc.incomplete)
{
#ifdef UNROLLED_ARC_LOOP
// Unrolling the arc code is fast, but costs about 830 bytes of extra code space.
int q = quadrant(state.arc.x, state.arc.y);
if (state.arc.angular_direction) {
switch (q) {
case 0:
map_local_arc_directions_to_stepper_directions(1,-1);
while(state.mode && (state.arc.x>state.arc.y)) { step_arc_along_x(1,-1); }
while(state.arc.incomplete && (state.arc.x>state.arc.y)) { step_arc_along_x(1,-1); }
case 1:
map_local_arc_directions_to_stepper_directions(1,-1);
while(state.mode && (state.arc.y>0)) { step_arc_along_y(1,-1); }
while(state.arc.incomplete && (state.arc.y>0)) { step_arc_along_y(1,-1); }
case 2:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(state.mode && (state.arc.y>-state.arc.x)) { step_arc_along_y(-1,-1); }
while(state.arc.incomplete && (state.arc.y>-state.arc.x)) { step_arc_along_y(-1,-1); }
case 3:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(state.mode && (state.arc.x>0)) { step_arc_along_x(-1,-1); }
while(state.arc.incomplete && (state.arc.x>0)) { step_arc_along_x(-1,-1); }
case 4:
map_local_arc_directions_to_stepper_directions(-1,1);
while(state.mode && (state.arc.y<state.arc.x)) { step_arc_along_x(-1,1); }
while(state.arc.incomplete && (state.arc.y<state.arc.x)) { step_arc_along_x(-1,1); }
case 5:
map_local_arc_directions_to_stepper_directions(-1,1);
while(state.mode && (state.arc.y<0)) { step_arc_along_y(-1,1); }
while(state.arc.incomplete && (state.arc.y<0)) { step_arc_along_y(-1,1); }
case 6:
map_local_arc_directions_to_stepper_directions(1,-1);
while(state.mode && (state.arc.y<-state.arc.x)) { step_arc_along_y(1,1); }
while(state.arc.incomplete && (state.arc.y<-state.arc.x)) { step_arc_along_y(1,1); }
case 7:
map_local_arc_directions_to_stepper_directions(1,1);
while(state.mode && (state.arc.x<0)) { step_arc_along_x(1,1); }
while(state.arc.incomplete && (state.arc.x<0)) { step_arc_along_x(1,1); }
}
} else {
switch (q) {
case 7:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(state.mode && (state.arc.y>-state.arc.x)) { step_arc_along_x(-1,-1); }
while(state.arc.incomplete && (state.arc.y>-state.arc.x)) { step_arc_along_x(-1,-1); }
case 6:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(state.mode && (state.arc.y>0)) { step_arc_along_y(-1,-1); }
while(state.arc.incomplete && (state.arc.y>0)) { step_arc_along_y(-1,-1); }
case 5:
map_local_arc_directions_to_stepper_directions(1,-1);
while(state.mode && (state.arc.y>state.arc.x)) { step_arc_along_y(1,-1); }
while(state.arc.incomplete && (state.arc.y>state.arc.x)) { step_arc_along_y(1,-1); }
case 4:
map_local_arc_directions_to_stepper_directions(1,-1);
while(state.mode && (state.arc.x<0)) { step_arc_along_x(1,-1); }
while(state.arc.incomplete && (state.arc.x<0)) { step_arc_along_x(1,-1); }
case 3:
map_local_arc_directions_to_stepper_directions(1,1);
while(state.mode && (state.arc.y<-state.arc.x)) { step_arc_along_x(1,1); }
while(state.arc.incomplete && (state.arc.y<-state.arc.x)) { step_arc_along_x(1,1); }
case 2:
map_local_arc_directions_to_stepper_directions(1,1);
while(state.mode && (state.arc.y<0)) { step_arc_along_y(1,1); }
while(state.arc.incomplete && (state.arc.y<0)) { step_arc_along_y(1,1); }
case 1:
map_local_arc_directions_to_stepper_directions(-1,1);
while(state.mode && (state.arc.y<state.arc.x)) { step_arc_along_y(-1,1); }
while(state.arc.incomplete && (state.arc.y<state.arc.x)) { step_arc_along_y(-1,1); }
case 0:
map_local_arc_directions_to_stepper_directions(-1,1);
while(state.mode && (state.arc.x>0)) { step_arc_along_x(-1,1); }
while(state.arc.incomplete && (state.arc.x>0)) { step_arc_along_x(-1,1); }
}
}
#else
dx = (state.arc.y!=0) ? sign(state.arc.y) * state.arc.angular_direction : -sign(state.arc.x);
dy = (state.arc.x!=0) ? -sign(state.arc.x) * state.arc.angular_direction : -sign(state.arc.y);
if (fabs(state.arc.x)<fabs(state.arc.y)) {
step_arc_along_x(dx,dy);
} else {
step_arc_along_y(dx,dy);
}
#endif
}
// Update the tool position to the new actual position
state.position[state.arc.axis_x] += state.arc.x-start_x;
state.position[state.arc.axis_y] += state.arc.y-start_y;
// Because of rounding errors we might be off by a step or two. Adjust for this
// To be implemented
//void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate)
}
void mc_go_home()