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

optimized for size, shaved 2k

Browse Source
This commit is contained in:
Simen Svale Skogsrud 2009-02-08 21:22:54 +01:00
parent c07a322589
commit 05bacc436e
5 changed files with 202 additions and 258 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gcode.c
View File

@ -52,6 +52,7 @@
#include "spindle_control.h" #include "spindle_control.h"
#include "geometry.h" #include "geometry.h"
#include "errno.h" #include "errno.h"
#include "serial_protocol.h"
#include "wiring_serial.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). // characters and signed floats (no whitespace).
uint8_t gc_execute_line(char *line) { uint8_t gc_execute_line(char *line) {
int counter = 0; int counter = 0;
int requires_nudge = false;
char letter; char letter;
double value; double value;
double unit_converted_value; double unit_converted_value;
@ -238,6 +238,7 @@ uint8_t gc_execute_line(char *line) {
} }
// Perform any physical actions // Perform any physical actions
sp_send_execution_marker();
switch (next_action) { switch (next_action) {
case NEXT_ACTION_GO_HOME: mc_go_home(); break; case NEXT_ACTION_GO_HOME: mc_go_home(); break;
case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); 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_CANCEL: break;
case MOTION_MODE_RAPID_LINEAR: case MOTION_MODE_LINEAR: case MOTION_MODE_RAPID_LINEAR: case MOTION_MODE_LINEAR:
if (gc.inverse_feed_rate_mode) { 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); inverse_feed_rate, true);
} else { } 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, (gc.motion_mode == MOTION_MODE_LINEAR) ? gc.feed_rate : RAPID_FEEDRATE,
false); false);
} }
@ -383,19 +384,10 @@ uint8_t gc_execute_line(char *line) {
// printInteger(trunc(radius)); // printInteger(trunc(radius));
// printByte(')'); // printByte(')');
mc_arc(theta_start, angular_travel, radius, gc.plane_axis_0, gc.plane_axis_1, gc.feed_rate); 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; 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 // 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 // motion control system might still be processing the action and the real tool position
// in any intermediate location. // in any intermediate location.

419
motion_control.c
View File

@ -40,17 +40,117 @@
#define ONE_MINUTE_OF_MICROSECONDS 60000000.0 #define ONE_MINUTE_OF_MICROSECONDS 60000000.0
// Parameters when mode is MC_MODE_ARC int8_t mode; // The current operation mode
struct LinearMotionParameters { 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) 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 int32_t target[3], // The target position in absolute steps
step_count[3], // Absolute steps of travel along each axis step_count[3], // Absolute steps of travel along each axis
counter[3], // A counter used in the bresenham algorithm for line plotting 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
};
struct ArcMotionParameters { 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;
int8_t direction[3]; // The direction of travel along each axis (-1, 0 or 1) int8_t direction[3]; // The direction of travel along each axis (-1, 0 or 1)
int8_t angular_direction; // 1 = clockwise, -1 = anticlockwise int8_t angular_direction; // 1 = clockwise, -1 = anticlockwise
int32_t x, y, target_x, target_y; // current position and target position in the 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), int32_t error, x2, y2; // error is always == (x**2 + y**2 - radius**2),
// x2 is always 2*x, y2 is always 2*y // x2 is always 2*x, y2 is always 2*y
uint8_t axis_x, axis_y; // maps the arc axes to stepper axes 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 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 int dx, dy; // Trace directions
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;
}
}
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); uint32_t radius_steps = round(radius*X_STEPS_PER_MM);
if(radius_steps == 0) { return; } if(radius_steps == 0) { return; }
mc.mode = MC_MODE_ARC; mode = MC_MODE_ARC;
// Determine angular direction (+1 = clockwise, -1 = counterclockwise) // 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 // Calculate the initial position and target position in the local coordinate system of the arc
mc.arc.x = round(sin(theta)*radius_steps); start_x = x = round(sin(theta)*radius_steps);
mc.arc.y = round(cos(theta)*radius_steps); start_y = y = round(cos(theta)*radius_steps);
mc.arc.target_x = trunc(sin(theta+angular_travel)*radius_steps); target_x = trunc(sin(theta+angular_travel)*radius_steps);
mc.arc.target_y = trunc(cos(theta+angular_travel)*radius_steps); target_y = trunc(cos(theta+angular_travel)*radius_steps);
// Precalculate these values to optimize target detection // Precalculate these values to optimize target detection
mc.arc.target_direction_x = signof(mc.arc.target_x)*mc.arc.angular_direction; target_direction_x = signof(target_x)*angular_direction;
mc.arc.target_direction_y = signof(mc.arc.target_y)*mc.arc.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 // 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 // <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. // 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 // 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. // by keeping track of the doubles of the arc coordinates at all times.
mc.arc.x2 = 2*mc.arc.x; x2 = 2*x;
mc.arc.y2 = 2*mc.arc.y; y2 = 2*y;
// Set up a vector with the steppers we are going to use tracing the plane of this arc // 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; diagonal_bits = st_bit_for_stepper(axis_1);
mc.arc.plane_steppers[axis_2] = 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 // And map the local coordinate system of the arc onto the tool axes of the selected plane
mc.arc.axis_x = axis_1; axis_x = axis_1;
mc.arc.axis_y = axis_2; axis_y = axis_2;
// The amount of steppings performed while tracing a full circle is equal to the sum of sides in a // 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: // 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))); 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; double millimeters_half_circumference = radius*M_PI;
// Then we calculate the microseconds between each step as if we will trace the full circle. // 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. // It doesn't matter what fraction of the circle we are actuallyt going to trace. The pace is the same.
mc.pace = st_buffer_pace(((millimeters_half_circumference * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / steps_in_half_circle);
((millimeters_half_circumference * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / steps_in_half_circle;
mc.arc.incomplete = true;
}
#define check_arc_target \ incomplete = true;
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 // Execution
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 while(incomplete)
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)
{ {
dx = (mc.arc.y!=0) ? signof(mc.arc.y) * mc.arc.angular_direction : -signof(mc.arc.x); dx = (y!=0) ? signof(y) * angular_direction : -signof(x);
dy = (mc.arc.x!=0) ? -signof(mc.arc.x) * mc.arc.angular_direction : -signof(mc.arc.y); 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 // 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. // selected tool-space-axes for the current arc.
mc.arc.direction[mc.arc.axis_x] = dx; direction[axis_x] = dx;
mc.arc.direction[mc.arc.axis_y] = dy; direction[axis_y] = dy;
set_stepper_directions(mc.arc.direction); set_stepper_directions(direction);
if (abs(mc.arc.x)<abs(mc.arc.y)) { if (abs(x)<abs(y)) {
step_arc_along_x(dx,dy); // Step arc horizontally
x+=dx;
error += 1+x2*dx;
x2 += 2*dx;
diagonal_error = error + 1 + y2*dy;
if(abs(error) >= abs(diagonal_error)) {
y += dy;
y2 += 2*dy;
error = diagonal_error;
step_steppers(diagonal_bits); // step diagonal
} else { } else {
step_arc_along_y(dx,dy); step_axis(axis_x); // step straight
} }
} else {
// 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 // Update the tool position to the new actual position
mc.position[mc.arc.axis_x] += mc.arc.x-start_x; position[axis_x] += x-start_x;
mc.position[mc.arc.axis_y] += mc.arc.y-start_y; position[axis_y] += y-start_y;
mc.mode = MC_MODE_AT_REST; mode = MC_MODE_AT_REST;
} }
void mc_go_home() void mc_go_home()
{ {
mc.mode = MC_MODE_HOME; mode = MC_MODE_HOME;
}
void execute_go_home()
{
st_go_home(); st_go_home();
st_synchronize(); st_synchronize();
clear_vector(mc.position); // By definition this is location [0, 0, 0] clear_vector(position); // By definition this is location [0, 0, 0]
mc.mode = MC_MODE_AT_REST; 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;
}
}
}
} }
int mc_status() int mc_status()
{ {
return(mc.mode); return(mode);
} }
// Set the direction pins for the stepper motors according to the provided vector. // 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 way we can generate the whole direction bit-mask without doing any comparisions
or branching. Fast and compact, yet practically unreadable. Sorry sorry sorry. 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[X_AXIS]&0x80)>>(7-X_DIRECTION_BIT)) |
((direction[Y_AXIS]&0x80)>>(7-Y_DIRECTION_BIT)) | ((direction[Y_AXIS]&0x80)>>(7-Y_DIRECTION_BIT)) |
((direction[Z_AXIS]&0x80)>>(7-Z_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 // 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 // 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. // 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]<<X_STEP_BIT) | st_buffer_step(direction_bits | bits);
(enabled[Y_AXIS]<<Y_STEP_BIT) | (enabled[Z_AXIS]<<Z_STEP_BIT));
} }
// Step only one motor // Step only one motor
inline void step_axis(uint8_t axis) inline void step_axis(uint8_t axis)
{ {
switch (axis) { switch (axis) {
case X_AXIS: st_buffer_step(mc.direction_bits | (1<<X_STEP_BIT)); break; case X_AXIS: st_buffer_step(direction_bits | (1<<X_STEP_BIT)); break;
case Y_AXIS: st_buffer_step(mc.direction_bits | (1<<Y_STEP_BIT)); break; case Y_AXIS: st_buffer_step(direction_bits | (1<<Y_STEP_BIT)); break;
case Z_AXIS: st_buffer_step(mc.direction_bits | (1<<Z_STEP_BIT)); break; case Z_AXIS: st_buffer_step(direction_bits | (1<<Z_STEP_BIT)); break;
} }
} }

2
motion_control.h
View File

@ -34,7 +34,7 @@ void mc_init();
// Prepare for linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // 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. // 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); 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, // 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 // positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the

12
stepper.c
View File

@ -108,7 +108,8 @@ void st_init()
void st_buffer_step(uint8_t motor_port_bits) void st_buffer_step(uint8_t motor_port_bits)
{ {
if (echo_steps) { if (echo_steps && !(motor_port_bits&0x80)) {
// Echo steps. If bit 7 is set, the message is internal to Grbl and should not be echoed
printByte('!'+motor_port_bits); printByte('!'+motor_port_bits);
} }
@ -170,6 +171,15 @@ void st_buffer_pace(uint32_t microseconds)
st_buffer_step(0xff); st_buffer_step(0xff);
} }
uint8_t st_bit_for_stepper(int axis) {
switch(axis) {
case X_AXIS: return(1<<X_STEP_BIT);
case Y_AXIS: return(1<<Y_STEP_BIT);
case Z_AXIS: return(1<<Z_STEP_BIT);
}
return(0);
}
void config_pace_timer(uint32_t microseconds) void config_pace_timer(uint32_t microseconds)
{ {
uint32_t ticks = microseconds*TICKS_PER_MICROSECOND; uint32_t ticks = microseconds*TICKS_PER_MICROSECOND;

7
stepper.h
View File

@ -32,8 +32,11 @@
// Initialize and start the stepper motor subsystem // Initialize and start the stepper motor subsystem
void st_init(); void st_init();
// Set the rate steps are taken from the buffer and executed // Returns a bitmask with the stepper bit for the given axis set
void st_set_pace(uint32_t microseconds); uint8_t st_bit_for_stepper(int axis);
// Buffer a change in the rate steps are taken from the buffer and executed
void st_buffer_pace(uint32_t microseconds);
// Buffer a new instruction for the steppers // Buffer a new instruction for the steppers
void st_buffer_step(uint8_t motor_port_bits); void st_buffer_step(uint8_t motor_port_bits);