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

support for helical motion

Browse Source
This commit is contained in:
Simen Svale Skogsrud 2009-02-11 00:37:33 +01:00
parent 8f3a69b37e
commit e257fc195c
6 changed files with 164 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
gcode.c
View File

@ -89,7 +89,7 @@ struct ParserState {
double position[3]; /* Where the interpreter considers the tool to be at this point in the code */
uint8_t tool;
int16_t spindle_speed; /* RPM/100 */
uint8_t plane_axis_0, plane_axis_1; // The axes of the selected plane
uint8_t plane_axis_0, plane_axis_1, plane_axis_2; // The axes of the selected plane
};
struct ParserState gc;
@ -103,21 +103,23 @@ int read_double(char *line, //!< string: line of RS274/NGC code being processed
int next_statement(char *letter, double *double_ptr, char *line, int *counter);
void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2)
{
gc.plane_axis_0 = axis_0;
gc.plane_axis_1 = axis_1;
gc.plane_axis_2 = axis_2;
}
void gc_init() {
memset(&gc, 0, sizeof(gc));
gc.feed_rate = DEFAULT_FEEDRATE;
gc.plane_axis_0 = X_AXIS; gc.plane_axis_1 = Y_AXIS;
select_plane(X_AXIS, Y_AXIS, Z_AXIS);
}
inline float to_millimeters(double value) {
return(gc.inches_mode ? value * INCHES_PER_MM : value);
}
void select_plane(uint8_t axis_0, uint8_t axis_1)
{
gc.plane_axis_0 = axis_0;
gc.plane_axis_1 = axis_1;
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floats (no whitespace).
@ -159,9 +161,9 @@ uint8_t gc_execute_line(char *line) {
case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
case 4: next_action = NEXT_ACTION_DWELL; break;
case 17: select_plane(X_AXIS, Y_AXIS); break;
case 18: select_plane(X_AXIS, Z_AXIS); break;
case 19: select_plane(Y_AXIS, Z_AXIS); break;
case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
case 20: gc.inches_mode = true; break;
case 21: gc.inches_mode = false; break;
case 28: case 30: next_action = NEXT_ACTION_GO_HOME; break;
@ -306,6 +308,7 @@ uint8_t gc_execute_line(char *line) {
// Calculate the change in position along each selected axis
double x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
double y = target[gc.plane_axis_1]-gc.position[gc.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)
// If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any
@ -371,8 +374,14 @@ uint8_t gc_execute_line(char *line) {
}
// Find the radius
double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]);
// Calculate the motion along the depth axis of the helix
double depth = target[gc.plane_axis_2]-gc.position[gc.plane_axis_2];
// Trace the arc
mc_arc(theta_start, angular_travel, radius, gc.plane_axis_0, gc.plane_axis_1, gc.feed_rate);
if (gc.inverse_feed_rate_mode) {
mc_arc(theta_start, angular_travel, radius, depth, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2, inverse_feed_rate, true);
} else {
mc_arc(theta_start, angular_travel, radius, depth, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2, gc.feed_rate, false);
}
break;
}
}

2
gcode.h
View File

@ -37,6 +37,6 @@ void gc_init();
uint8_t gc_execute_line(char *line);
// get the current logical position (in current units), the current status code and the unit mode
void gc_get_status(double *position, uint8_t *status_code, int *inches_mode, uint32_t *line_number);
void gc_get_status(double *position_, uint8_t *status_code_, int *inches_mode_, uint32_t *line_number_);
#endif

214
motion_control.c
View File

@ -63,8 +63,21 @@ void mc_dwell(uint32_t milliseconds)
mode = MC_MODE_AT_REST;
}
// 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
void set_step_pace(double feed_rate, double millimeters_of_travel, uint32_t steps, int invert) {
int32_t pace;
if (invert) {
pace = round(ONE_MINUTE_OF_MICROSECONDS/feed_rate/steps);
} else {
pace = round(((millimeters_of_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / steps);
}
st_buffer_pace(pace);
}
// Execute 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.
// unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
// 1/feed_rate minutes.
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
@ -74,14 +87,13 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_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
maximum_steps; // The larges absolute step-count of any axis
// Setup ---------------------------------------------------------------------------------------------------
target[X_AXIS] = x*X_STEPS_PER_MM;
target[Y_AXIS] = y*Y_STEPS_PER_MM;
target[Z_AXIS] = z*Z_STEPS_PER_MM;
target[Z_AXIS] = z*Z_STEPS_PER_MM;
// Determine direction and travel magnitude for each axis
for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
step_count[axis] = abs(target[axis] - position[axis]);
@ -98,25 +110,20 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
}
// 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);
}
// Ask old Phytagoras to estimate how many mm our next move is going to take us
double millimeters_of_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));
// And set the step pace
set_step_pace(feed_rate, millimeters_of_travel, maximum_steps, invert_feed_rate);
// Execution -----------------------------------------------------------------------------------------------
mode = MC_MODE_LINEAR;
while(mode) {
do {
// Trace the line
step_bits = 0;
for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
@ -131,23 +138,23 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
}
}
}
if (step_bits) {
step_steppers(step_bits);
} else {
mode = MC_MODE_AT_REST;
}
}
if(step_bits) { step_steppers(step_bits); }
} while (step_bits);
mode = MC_MODE_AT_REST;
}
// Execute 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.
// circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining
// axis in axis_l which will be the axis for linear travel if you are tracing a helical motion.
// 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)
void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2,
int axis_linear, double feed_rate, int invert_feed_rate)
{
uint32_t start_x, start_y;
uint32_t diagonal_error;
uint32_t start_x, start_y; // The start position in the coordinate system local to the circle
uint32_t diagonal_error; // A variable to keep track of varations in the error-value during
// the tracing of the arc
int8_t direction[3]; // The direction of travel along each axis (-1, 0 or 1)
int8_t angular_direction; // 1 = clockwise, -1 = anticlockwise
@ -156,19 +163,13 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
// center of the arc.
int target_direction_x, target_direction_y; // signof(target_x)*angular_direction precalculated for speed
int32_t error; // error is always == (x**2 + y**2 - radius**2),
uint8_t axis_x, axis_y; // maps the arc axes to stepper axes
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
int dx, dy; // Trace directions
// Setup
uint32_t radius_steps = round(radius*X_STEPS_PER_MM);
if(radius_steps == 0) { return; }
int dx, dy; // Trace directions
// Setup arc interpolation --------------------------------------------------------------------------------
uint32_t radius_steps = round(radius*X_STEPS_PER_MM);
if(radius_steps == 0) { return; }
// Determine angular direction (+1 = clockwise, -1 = counterclockwise)
angular_direction = signof(angular_travel);
// Calculate the initial position and target position in the local coordinate system of the arc
@ -183,12 +184,6 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
// <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.
error = x*x + y*y - radius_steps*radius_steps;
// Set up a vector with the steppers we are going to use tracing the plane of this arc
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
axis_x = axis_1;
axis_y = axis_2;
// Estimate length of arc in steps -------------------------------------------------------------------------
@ -210,95 +205,126 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
+---- 2 ----+
*/
// Find the quadrants of the starting point and the target
int start_quadrant = quadrant_of_the_circle(start_x, start_y);
int target_quadrant = quadrant_of_the_circle(target_x, target_y);
uint32_t steps_to_travel=0;
// Is the start and target point in the same quadrant?
uint32_t arc_steps=0;
// Will this whole arc take place within the same quadrant?
if (start_quadrant == target_quadrant && (abs(angular_travel) <= (M_PI/2))) {
if(quadrant_horizontal(start_quadrant)) { // a horizontal quadrant where x will be the primary direction
steps_to_travel = abs(target_x-start_x);
arc_steps = abs(target_x-start_x);
} else { // a vertical quadrant where y will be the primary direction
steps_to_travel = abs(target_y-start_y);
arc_steps = abs(target_y-start_y);
}
} else { // the start and target points are in different quadrants
// Lets estimate the amount of steps along one full quadrant
// Lets estimate the amount of steps along half a quadrant
uint32_t steps_in_half_quadrant = ceil(radius_steps/sqrt(2));
// Add the steps in the first partial quadrant
steps_to_travel += steps_in_partial_quadrant(start_x, start_y,
arc_steps += steps_in_partial_quadrant(start_x, start_y,
start_quadrant, angular_direction, steps_in_half_quadrant);
// Count the number of full quadrants between the start and end quadrants
uint8_t full_quadrants_traveled = full_quadrants_between(start_quadrant, target_quadrant, angular_direction);
// Add steps for the full quadrants plus some stray steps for "corners"
steps_to_travel += full_quadrants_traveled*(steps_in_half_quadrant*2+1);
arc_steps += full_quadrants_traveled*(steps_in_half_quadrant*2+1);
// Add the steps in the final partial quadrant. By inverting the angular direction we get the correct number for
// the target quadrant which steps through the opposite part of the quadrant with respect to the start quadrant.
steps_to_travel += steps_in_partial_quadrant(target_x, target_y,
arc_steps += steps_in_partial_quadrant(target_x, target_y,
target_quadrant, -angular_direction, steps_in_half_quadrant);
}
// Set up the linear interpolation of the "depth" axis -----------------------------------------------------
int32_t linear_steps = abs(st_millimeters_to_steps(linear_travel, axis_linear));
int linear_direction = signof(linear_travel);
// The number of steppings needed to trace this motion is equal to the motion that require the maximum
// amount of steps: the arc or the line:
int32_t maximum_steps = max(linear_steps, arc_steps);
// Initialize the counters to do linear bresenham
int32_t linear_counter = -maximum_steps/2;
int32_t arc_counter = -maximum_steps/2;
// Calculate feed rate -------------------------------------------------------------------------------------
// The amount of steppings performed while tracing a half 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 half circle:
uint32_t steps_in_half_circle = round((4*radius_steps)/sqrt(2));
// We then calculate the millimeters of travel along the circumference of that same half circle
double millimeters_half_circumference = radius*M_PI;
// We then calculate the millimeters of helical travel
double millimeters_of_travel = sqrt(pow(angular_travel*radius,2)+pow(abs(linear_travel),2));
// 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 actually going to trace. The pace is the same.
st_buffer_pace(((millimeters_half_circumference * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / steps_in_half_circle);
set_step_pace(feed_rate, millimeters_of_travel, maximum_steps, invert_feed_rate);
// Execution -----------------------------------------------------------------------------------------------
mode = MC_MODE_ARC;
direction[axis_linear] = linear_direction;
uint8_t axis_1_bit = st_bit_for_stepper(axis_1);
uint8_t axis_2_bit = st_bit_for_stepper(axis_2);
uint8_t axis_linear_bit = st_bit_for_stepper(axis_linear);
uint8_t diagonal_bits = (axis_1_bit | axis_2_bit);
incomplete = true;
while(incomplete)
uint8_t step_bits;
while(mode)
{
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.
direction[axis_x] = dx;
direction[axis_y] = dy;
set_stepper_directions(direction);
// Check which axis will be "major" for this stepping
if (abs(x)<abs(y)) {
// Step arc horizontally
error += 1 + 2*x * dx;
x+=dx;
diagonal_error = error + 1 + 2*y*dy;
if(abs(error) >= abs(diagonal_error)) {
y += dy;
error = diagonal_error;
step_steppers(diagonal_bits); // step diagonal
} else {
step_axis(axis_x); // step straight
}
} else {
// Step arc vertically
error += 1 + 2*y * dy;
y+=dy;
diagonal_error = error + 1 + 2*x * dx;
if(abs(error) >= abs(diagonal_error)) {
x += dx;
error = diagonal_error;
step_steppers(diagonal_bits); // step diagonal
} else {
step_axis(axis_y); // step straight
// reset step bits
step_bits = 0;
// Do linear interpolation
linear_counter += linear_steps;
if (linear_counter > 0) {
linear_counter -= maximum_steps;
step_bits |= axis_linear_bit;
}
// Do arc interpolation
arc_counter += arc_steps;
if (arc_counter > 0) {
arc_counter -= maximum_steps;
// Determine directions for each axis at this point in the arc
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.
direction[axis_1] = dx;
direction[axis_2] = dy;
// Check which axis will be "major" for this stepping
if (abs(x)<abs(y)) {
// X is major: Step arc horizontally
error += 1 + 2*x * dx;
x+=dx;
diagonal_error = error + 1 + 2*y*dy;
if(abs(error) >= abs(diagonal_error)) {
y += dy;
error = diagonal_error;
step_bits |= diagonal_bits; // step diagonal
} else {
step_bits |= axis_1_bit; // step straight
}
} else {
// Y is major: Step arc vertically
error += 1 + 2*y * dy;
y+=dy;
diagonal_error = error + 1 + 2*x * dx;
if(abs(error) >= abs(diagonal_error)) {
x += dx;
error = diagonal_error;
step_bits |= diagonal_bits; // step diagonal
} else {
step_bits |= axis_2_bit; // step straight
}
}
}
set_stepper_directions(direction);
step_steppers(step_bits);
// Check if target has been reached. Todo: Simplify/optimize/clarify
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; } }
{ mode = MC_MODE_AT_REST; } }
}
// Update the tool position to the new actual position
position[axis_x] += x-start_x;
position[axis_y] += y-start_y;
mode = MC_MODE_AT_REST;
position[axis_1] += x-start_x;
position[axis_2] += y-start_y;
position[axis_2] += linear_steps*linear_direction;
}
void mc_go_home()

20
motion_control.h
View File

@ -32,28 +32,26 @@
// Initializes the motion_control subsystem resources
void mc_init();
// 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.
// Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
// unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
// 1/feed_rate minutes.
void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate);
// 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.
// Known issue: This method pretends that all axes uses the same steps/mm as the X axis. Which might
// not be the case ... (To be continued)
void mc_arc(double theta, double angular_travel, double radius, int axis_1, int axis_2, double feed_rate);
// not be the case ... (To be continued)
// Regarding feed rate see note on mc_line.
void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2,
int axis_linear, double feed_rate, int invert_feed_rate);
// Prepare linear motion relative to the current position.
// Dwell for a couple of time units
void mc_dwell(uint32_t milliseconds);
// Prepare to send the tool position home
// Send the tool home
void mc_go_home();
// Start the prepared operation. In the current implementation this will block for most of the task at hand.
// In future implementations it might not block at all. If you want to make sure the system has reached
// quiescence call mc_wait()
void mc_execute();
// Check motion control status. result == 0: the system is idle. result > 0: the system is busy,
// result < 0: the system is in an error state.
int mc_status();

11
stepper.c
View File

@ -24,6 +24,7 @@
#include "stepper.h"
#include "config.h"
#include <math.h>
#include "nuts_bolts.h"
#include <avr/interrupt.h>
@ -286,3 +287,13 @@ void st_set_echo(int value)
{
echo_steps = value;
}
// Convert from millimeters to step-counts along the designated axis
int32_t st_millimeters_to_steps(double millimeters, int axis) {
switch(axis) {
case X_AXIS: return(round(millimeters*X_STEPS_PER_MM));
case Y_AXIS: return(round(millimeters*Y_STEPS_PER_MM));
case Z_AXIS: return(round(millimeters*Z_STEPS_PER_MM));
}
return(0);
}

3
stepper.h
View File

@ -62,4 +62,7 @@ void st_go_home();
// Echo steps to serial port? (true/false)
void st_set_echo(int value);
// Convert from millimeters to step-counts along the designated axis
int32_t st_millimeters_to_steps(double millimeters, int axis);
#endif