From 9799955555bdde74962bb7fe1f4caa5e78dce632 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sun, 1 Feb 2009 11:58:21 +0100 Subject: [PATCH] arc code complete with support for both R and IJK style blocks --- arc_algorithm/arc.rb | 174 +++---------------------------------------- config.h | 3 + gcode.c | 111 ++++++++++++++++++++++----- geometry.c | 4 - geometry.h | 3 - motion_control.c | 79 ++++++++++++++------ 6 files changed, 161 insertions(+), 213 deletions(-) diff --git a/arc_algorithm/arc.rb b/arc_algorithm/arc.rb index 31eb975..7bb8db4 100644 --- a/arc_algorithm/arc.rb +++ b/arc_algorithm/arc.rb @@ -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.abs0 # 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) diff --git a/config.h b/config.h index 05ae101..8b2594e 100644 --- a/config.h +++ b/config.h @@ -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 diff --git a/gcode.c b/gcode.c index 48466c6..11e7acd 100644 --- a/gcode.c +++ b/gcode.c @@ -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; } } diff --git a/geometry.c b/geometry.c index 3b9c27e..fa387de 100644 --- a/geometry.c +++ b/geometry.c @@ -36,7 +36,3 @@ double theta(double x, double y) } } -double hypot(double x, double y) -{ - sqrt(x*x + y*y); -} diff --git a/geometry.h b/geometry.h index 460d966..ba06dc6 100644 --- a/geometry.h +++ b/geometry.h @@ -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 \ No newline at end of file diff --git a/motion_control.c b/motion_control.c index 97e773f..50cf1af 100644 --- a/motion_control.c +++ b/motion_control.c @@ -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 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.y0)) { 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)