From 05bacc436e7ede1a760aac4f064d81e74af8b7c1 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sun, 8 Feb 2009 21:22:54 +0100 Subject: [PATCH] optimized for size, shaved 2k --- gcode.c | 16 +- motion_control.c | 423 ++++++++++++++++++++--------------------------- motion_control.h | 2 +- stepper.c | 12 +- stepper.h | 7 +- 5 files changed, 202 insertions(+), 258 deletions(-) diff --git a/gcode.c b/gcode.c index 55d3dae..c175f23 100644 --- a/gcode.c +++ b/gcode.c @@ -52,6 +52,7 @@ #include "spindle_control.h" #include "geometry.h" #include "errno.h" +#include "serial_protocol.h" #include "wiring_serial.h" @@ -126,7 +127,6 @@ void select_plane(uint8_t axis_0, uint8_t axis_1) // characters and signed floats (no whitespace). uint8_t gc_execute_line(char *line) { int counter = 0; - int requires_nudge = false; char letter; double value; double unit_converted_value; @@ -238,6 +238,7 @@ uint8_t gc_execute_line(char *line) { } // Perform any physical actions + sp_send_execution_marker(); switch (next_action) { case NEXT_ACTION_GO_HOME: mc_go_home(); break; case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break; @@ -246,10 +247,10 @@ uint8_t gc_execute_line(char *line) { case MOTION_MODE_CANCEL: break; case MOTION_MODE_RAPID_LINEAR: case MOTION_MODE_LINEAR: if (gc.inverse_feed_rate_mode) { - mc_linear_motion(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], + mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], inverse_feed_rate, true); } else { - mc_linear_motion(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], + mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], (gc.motion_mode == MOTION_MODE_LINEAR) ? gc.feed_rate : RAPID_FEEDRATE, false); } @@ -383,19 +384,10 @@ uint8_t gc_execute_line(char *line) { // printInteger(trunc(radius)); // printByte(')'); mc_arc(theta_start, angular_travel, radius, gc.plane_axis_0, gc.plane_axis_1, gc.feed_rate); - // Rounding errors means the arcing might not land us exactly where we wanted. Thats why this - // operation must be finalized with a linear nudge to the exact target spot. - requires_nudge = true; break; } } - mc_execute(); - if (requires_nudge) { - mc_linear_motion(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.feed_rate, false); - mc_execute(); - } - // As far as the parser is concerned, the position is now == target. In reality the // motion control system might still be processing the action and the real tool position // in any intermediate location. diff --git a/motion_control.c b/motion_control.c index 617e44a..15e3425 100644 --- a/motion_control.c +++ b/motion_control.c @@ -40,17 +40,117 @@ #define ONE_MINUTE_OF_MICROSECONDS 60000000.0 -// Parameters when mode is MC_MODE_ARC -struct LinearMotionParameters { +int8_t mode; // The current operation mode +int32_t position[3]; // The current position of the tool in absolute steps +uint8_t direction_bits; // The direction bits to be used with any upcoming step-instruction + +void set_stepper_directions(int8_t *direction); +inline void step_steppers(uint8_t bits); +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() +{ + mode = 0; + clear_vector(position); +} + +void mc_dwell(uint32_t milliseconds) +{ + mode = MC_MODE_DWELL; + st_synchronize(); + _delay_ms(milliseconds); + mode = MC_MODE_AT_REST; +} + +// Prepare for linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second +// unless invert_feed_rate is true. Then the feed_rate states the number of seconds for the whole movement. +void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate) +{ + // Flags to keep track of which axes to step + uint8_t step_bits; + uint8_t axis; // loop variable int8_t direction[3]; // The direction of travel along each axis (-1, 0 or 1) - uint16_t feed_rate; int32_t target[3], // The target position in absolute steps step_count[3], // Absolute steps of travel along each axis counter[3], // A counter used in the bresenham algorithm for line plotting maximum_steps; // The larges absolute step-count of any axis -}; + + target[X_AXIS] = x*X_STEPS_PER_MM; + target[Y_AXIS] = y*Y_STEPS_PER_MM; + target[Z_AXIS] = z*Z_STEPS_PER_MM; + + mode = MC_MODE_LINEAR; + + // Determine direction and travel magnitude for each axis + for(axis = X_AXIS; axis <= Z_AXIS; axis++) { + step_count[axis] = abs(target[axis] - position[axis]); + direction[axis] = signof(target[axis] - position[axis]); + } + // Find the magnitude of the axis with the longest travel + maximum_steps = max(step_count[Z_AXIS], + max(step_count[X_AXIS], step_count[Y_AXIS])); + // Nothing to do? + if (maximum_steps == 0) + { + mode = MC_MODE_AT_REST; + return; + } + // Set up a neat counter for each axis + for(axis = X_AXIS; axis <= Z_AXIS; axis++) { + counter[axis] = -maximum_steps/2; + } + // Set our direction pins + set_stepper_directions(direction); + // Calculate the microseconds we need to wait between each step to achieve the desired feed rate + if (invert_feed_rate) { + st_buffer_pace((feed_rate*1000000)/maximum_steps); + } else { + // Ask old Phytagoras to estimate how many mm our next move is going to take us: + double millimeters_to_travel = + sqrt(pow(X_STEPS_PER_MM*step_count[X_AXIS],2) + + pow(Y_STEPS_PER_MM*step_count[Y_AXIS],2) + + pow(Z_STEPS_PER_MM*step_count[Z_AXIS],2)); + // Calculate the microseconds between steps that we should wait in order to travel the + // designated amount of millimeters in the amount of steps we are going to generate + st_buffer_pace(((millimeters_to_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / maximum_steps); + } + + // Execution + + while(mode) { + // Trace the line + step_bits = 0; + for(axis = X_AXIS; axis <= Z_AXIS; axis++) { + if (target[axis] != position[axis]) + { + counter[axis] += step_count[axis]; + if (counter[axis] > 0) + { + step_bits |= st_bit_for_stepper(axis); + counter[axis] -= maximum_steps; + position[axis] += direction[axis]; + } + } + } + if (step_bits) { + step_steppers(step_bits); + } else { + mode = MC_MODE_AT_REST; + } + } +} + + +// Prepare an arc. theta == start angle, angular_travel == number of radians to go along the arc, +// positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the +// circle in millimeters. axis_1 and axis_2 selects the plane in tool space. +// ISSUE: The arc interpolator assumes all axes have the same steps/mm as the X axis. +void mc_arc(double theta, double angular_travel, double radius, int axis_1, int axis_2, double feed_rate) +{ + uint32_t start_x, start_y; + uint32_t diagonal_error; -struct ArcMotionParameters { int8_t direction[3]; // The direction of travel along each axis (-1, 0 or 1) int8_t angular_direction; // 1 = clockwise, -1 = anticlockwise int32_t x, y, target_x, target_y; // current position and target position in the @@ -60,157 +160,38 @@ struct ArcMotionParameters { int32_t error, x2, y2; // error is always == (x**2 + y**2 - radius**2), // x2 is always 2*x, y2 is always 2*y uint8_t axis_x, axis_y; // maps the arc axes to stepper axes - int8_t plane_steppers[3]; // A vector with the steppers of axis_x and axis_y set to 1, the remaining 0 + int8_t diagonal_bits; // A bitmask with the stepper bits for both selected axes set 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 - read, but lets us initialize the state of the system by just clearing a single, contigous block of memory. - By overlaying the variables of the different modes in a union we save a few bytes of precious SRAM. -*/ -struct MotionControlState { - int8_t mode; // The current operation mode - int32_t position[3]; // The current position of the tool in absolute steps - int32_t pace; // Microseconds between each update in the current mode - uint8_t direction_bits; // The direction bits to be used with any upcoming step-instruction - union { - struct LinearMotionParameters linear; // variables used in MC_MODE_LINEAR - struct ArcMotionParameters arc; // variables used in MC_MODE_ARC - uint32_t dwell_milliseconds; // variable used in MC_MODE_DWELL - }; -}; -struct MotionControlState mc; - -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() -{ - // Initialize state variables - memset(&mc, 0, sizeof(mc)); -} - -void mc_dwell(uint32_t milliseconds) -{ - mc.mode = MC_MODE_DWELL; - mc.dwell_milliseconds = milliseconds; -} - -// Prepare for linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second -// 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) -{ - memset(&mc.linear, 0, sizeof(mc.arc)); - mc.linear.target[X_AXIS] = x*X_STEPS_PER_MM; - mc.linear.target[Y_AXIS] = y*Y_STEPS_PER_MM; - mc.linear.target[Z_AXIS] = z*Z_STEPS_PER_MM; - - mc.mode = MC_MODE_LINEAR; - uint8_t axis; // loop variable - // Determine direction and travel magnitude for each axis - for(axis = X_AXIS; axis <= Z_AXIS; axis++) { - mc.linear.step_count[axis] = abs(mc.linear.target[axis] - mc.position[axis]); - mc.linear.direction[axis] = signof(mc.linear.target[axis] - mc.position[axis]); - } - // Find the magnitude of the axis with the longest travel - mc.linear.maximum_steps = max(mc.linear.step_count[Z_AXIS], - max(mc.linear.step_count[X_AXIS], mc.linear.step_count[Y_AXIS])); - // Nothing to do? - if (mc.linear.maximum_steps == 0) - { - mc.mode = MC_MODE_AT_REST; - return; - } - // Set up a neat counter for each axis - for(axis = X_AXIS; axis <= Z_AXIS; axis++) { - mc.linear.counter[axis] = -mc.linear.maximum_steps/2; - } - // Set our direction pins - set_stepper_directions(mc.linear.direction); - // Calculate the microseconds we need to wait between each step to achieve the desired feed rate - if (invert_feed_rate) { - mc.pace = - (feed_rate*1000000)/mc.linear.maximum_steps; - } else { - // Ask old Phytagoras to estimate how many mm our next move is going to take us: - double millimeters_to_travel = - sqrt(pow(X_STEPS_PER_MM*mc.linear.step_count[X_AXIS],2) + - pow(Y_STEPS_PER_MM*mc.linear.step_count[Y_AXIS],2) + - pow(Z_STEPS_PER_MM*mc.linear.step_count[Z_AXIS],2)); - // Calculate the microseconds between steps that we should wait in order to travel the - // designated amount of millimeters in the amount of steps we are going to generate - mc.pace = - ((millimeters_to_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / mc.linear.maximum_steps; - } -} + int dx, dy; // Trace directions -void execute_linear_motion() -{ - // Flags to keep track of which axes to step - uint8_t step[3]; - uint8_t axis; // loop variable - - while(mc.mode) { - // Trace the line - clear_vector(step); - for(axis = X_AXIS; axis <= Z_AXIS; axis++) { - if (mc.linear.target[axis] != mc.position[axis]) - { - mc.linear.counter[axis] += mc.linear.step_count[axis]; - if (mc.linear.counter[axis] > 0) - { - step[axis] = true; - mc.linear.counter[axis] -= mc.linear.maximum_steps; - mc.position[axis] += mc.linear.direction[axis]; - } - } - } - if (step[X_AXIS] | step[Y_AXIS] | step[Z_AXIS]) { - step_steppers(step); - } else { - mc.mode = MC_MODE_AT_REST; - } - } -} - -// Prepare an arc. theta == start angle, angular_travel == number of radians to go along the arc, -// positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the -// circle in millimeters. axis_1 and axis_2 selects the plane in tool space. -// ISSUE: The arc interpolator assumes all axes have the same steps/mm as the X axis. -void mc_arc(double theta, double angular_travel, double radius, int axis_1, int axis_2, double feed_rate) -{ - memset(&mc.arc, 0, sizeof(mc.arc)); uint32_t radius_steps = round(radius*X_STEPS_PER_MM); if(radius_steps == 0) { return; } - mc.mode = MC_MODE_ARC; + mode = MC_MODE_ARC; // Determine angular direction (+1 = clockwise, -1 = counterclockwise) - mc.arc.angular_direction = signof(angular_travel); + angular_direction = signof(angular_travel); // Calculate the initial position and target position in the local coordinate system of the arc - mc.arc.x = round(sin(theta)*radius_steps); - mc.arc.y = round(cos(theta)*radius_steps); - mc.arc.target_x = trunc(sin(theta+angular_travel)*radius_steps); - mc.arc.target_y = trunc(cos(theta+angular_travel)*radius_steps); + start_x = x = round(sin(theta)*radius_steps); + start_y = y = round(cos(theta)*radius_steps); + target_x = trunc(sin(theta+angular_travel)*radius_steps); + target_y = trunc(cos(theta+angular_travel)*radius_steps); // Precalculate these values to optimize target detection - mc.arc.target_direction_x = signof(mc.arc.target_x)*mc.arc.angular_direction; - mc.arc.target_direction_y = signof(mc.arc.target_y)*mc.arc.angular_direction; + target_direction_x = signof(target_x)*angular_direction; + target_direction_y = signof(target_y)*angular_direction; // The "error" factor is kept up to date so that it is always == (x**2+y**2-radius**2). When error // <0 we are inside the arc, when it is >0 we are outside of the arc, and when it is 0 we // are exactly on top of the arc. - mc.arc.error = mc.arc.x*mc.arc.x + mc.arc.y*mc.arc.y - radius_steps*radius_steps; + error = x*x + y*y - radius_steps*radius_steps; // Because the error-value moves in steps of (+/-)2x+1 and (+/-)2y+1 we save a couple of multiplications // by keeping track of the doubles of the arc coordinates at all times. - mc.arc.x2 = 2*mc.arc.x; - mc.arc.y2 = 2*mc.arc.y; - + x2 = 2*x; + y2 = 2*y; // Set up a vector with the steppers we are going to use tracing the plane of this arc - mc.arc.plane_steppers[axis_1] = 1; - mc.arc.plane_steppers[axis_2] = 1; + diagonal_bits = st_bit_for_stepper(axis_1); + diagonal_bits |= st_bit_for_stepper(axis_2); // And map the local coordinate system of the arc onto the tool axes of the selected plane - mc.arc.axis_x = axis_1; - mc.arc.axis_y = axis_2; + axis_x = axis_1; + axis_y = axis_2; // The amount of steppings performed while tracing a full circle is equal to the sum of sides in a // square inscribed in the circle. We use this to estimate the amount of steps as if this arc was a full circle: uint32_t steps_in_half_circle = round(radius_steps * 4 * (1/sqrt(2))); @@ -218,121 +199,80 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int double millimeters_half_circumference = radius*M_PI; // Then we calculate the microseconds between each step as if we will trace the full circle. // It doesn't matter what fraction of the circle we are actuallyt going to trace. The pace is the same. - mc.pace = - ((millimeters_half_circumference * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / steps_in_half_circle; - mc.arc.incomplete = true; -} + st_buffer_pace(((millimeters_half_circumference * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / steps_in_half_circle); + + incomplete = true; -#define check_arc_target \ - if ((mc.arc.x * mc.arc.target_direction_y >= \ - mc.arc.target_x * mc.arc.target_direction_y) && \ - (mc.arc.y * mc.arc.target_direction_x <= \ - mc.arc.target_y * mc.arc.target_direction_x)) \ - { if ((signof(mc.arc.x) == signof(mc.arc.target_x)) && (signof(mc.arc.y) == signof(mc.arc.target_y))) \ - { mc.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) -{ - uint32_t diagonal_error; - mc.arc.x+=dx; - mc.arc.error += 1+mc.arc.x2*dx; - mc.arc.x2 += 2*dx; - diagonal_error = mc.arc.error + 1 + mc.arc.y2*dy; - if(abs(mc.arc.error) >= abs(diagonal_error)) { - mc.arc.y += dy; - mc.arc.y2 += 2*dy; - mc.arc.error = diagonal_error; - step_steppers(mc.arc.plane_steppers); // step diagonal - } else { - step_axis(mc.arc.axis_x); // step straight - } - check_arc_target; -} - -// Internal method used by execute_arc to trace vertically in the general direction provided by dx and dy -void step_arc_along_y(int8_t dx, int8_t dy) -{ - uint32_t diagonal_error; - mc.arc.y+=dy; - mc.arc.error += 1+mc.arc.y2*dy; - mc.arc.y2 += 2*dy; - diagonal_error = mc.arc.error + 1 + mc.arc.x2*dx; - if(abs(mc.arc.error) >= abs(diagonal_error)) { - mc.arc.x += dx; - mc.arc.x2 += 2*dx; - mc.arc.error = diagonal_error; - step_steppers(mc.arc.plane_steppers); // step diagonal - } else { - step_axis(mc.arc.axis_y); // step straight - } - check_arc_target; -} - -// Will trace the configured arc until the target is reached. -void execute_arc() -{ - uint32_t start_x = mc.arc.x; - uint32_t start_y = mc.arc.y; - int dx, dy; // Trace directions - - // mc.mode is set to 0 (MC_MODE_AT_REST) when target is reached - while(mc.arc.incomplete) + // Execution + + while(incomplete) { - dx = (mc.arc.y!=0) ? signof(mc.arc.y) * mc.arc.angular_direction : -signof(mc.arc.x); - dy = (mc.arc.x!=0) ? -signof(mc.arc.x) * mc.arc.angular_direction : -signof(mc.arc.y); + dx = (y!=0) ? signof(y) * angular_direction : -signof(x); + dy = (x!=0) ? -signof(x) * angular_direction : -signof(y); // Take dx and dy which are local to the arc being generated and map them on to the // selected tool-space-axes for the current arc. - mc.arc.direction[mc.arc.axis_x] = dx; - mc.arc.direction[mc.arc.axis_y] = dy; - set_stepper_directions(mc.arc.direction); + direction[axis_x] = dx; + direction[axis_y] = dy; + set_stepper_directions(direction); - if (abs(mc.arc.x)= abs(diagonal_error)) { + y += dy; + y2 += 2*dy; + error = diagonal_error; + step_steppers(diagonal_bits); // step diagonal + } else { + step_axis(axis_x); // step straight + } } else { - step_arc_along_y(dx,dy); + // Step arc vertically + y+=dy; + error += 1+y2*dy; + y2 += 2*dy; + diagonal_error = error + 1 + x2*dx; + if(abs(error) >= abs(diagonal_error)) { + x += dx; + x2 += 2*dx; + error = diagonal_error; + step_steppers(diagonal_bits); // step diagonal + } else { + step_axis(axis_y); // step straight + } } + + // Check if target has been reached + if ((x * target_direction_y >= + target_x * target_direction_y) && + (y * target_direction_x <= + target_y * target_direction_x)) + { if ((signof(x) == signof(target_x)) && (signof(y) == signof(target_y))) + { incomplete = false; } } } // Update the tool position to the new actual position - mc.position[mc.arc.axis_x] += mc.arc.x-start_x; - mc.position[mc.arc.axis_y] += mc.arc.y-start_y; - mc.mode = MC_MODE_AT_REST; + position[axis_x] += x-start_x; + position[axis_y] += y-start_y; + mode = MC_MODE_AT_REST; } void mc_go_home() { - mc.mode = MC_MODE_HOME; -} - -void execute_go_home() -{ + mode = MC_MODE_HOME; st_go_home(); st_synchronize(); - clear_vector(mc.position); // By definition this is location [0, 0, 0] - mc.mode = MC_MODE_AT_REST; -} - -void mc_execute() { - if (mc.mode != MC_MODE_AT_REST) { - st_buffer_pace(mc.pace); - sp_send_execution_marker(); - while(mc.mode) { // Loop because one task might start another task - switch(mc.mode) { - case MC_MODE_AT_REST: break; - case MC_MODE_DWELL: st_synchronize(); _delay_ms(mc.dwell_milliseconds); mc.mode = MC_MODE_AT_REST; break; - case MC_MODE_LINEAR: execute_linear_motion(); break; - case MC_MODE_ARC: execute_arc(); break; - case MC_MODE_HOME: execute_go_home(); break; - } - } - } + clear_vector(position); // By definition this is location [0, 0, 0] + mode = MC_MODE_AT_REST; } int mc_status() { - return(mc.mode); + return(mode); } // Set the direction pins for the stepper motors according to the provided vector. @@ -345,7 +285,7 @@ void set_stepper_directions(int8_t *direction) way we can generate the whole direction bit-mask without doing any comparisions or branching. Fast and compact, yet practically unreadable. Sorry sorry sorry. */ - mc.direction_bits = ( + direction_bits = ( ((direction[X_AXIS]&0x80)>>(7-X_DIRECTION_BIT)) | ((direction[Y_AXIS]&0x80)>>(7-Y_DIRECTION_BIT)) | ((direction[Z_AXIS]&0x80)>>(7-Z_DIRECTION_BIT))); @@ -354,19 +294,18 @@ void set_stepper_directions(int8_t *direction) // Step enabled steppers. Enabled should be an array of three bytes. Each byte represent one // stepper motor in the order X, Y, Z. Set the bytes of the steppers you want to step to // 1, and the rest to 0. -inline void step_steppers(uint8_t *enabled) +inline void step_steppers(uint8_t bits) { - st_buffer_step(mc.direction_bits | (enabled[X_AXIS]<