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

first stab at replacing step-buffering with line-buffering

Browse Source
This commit is contained in:
Simen Svale Skogsrud 2010-03-02 21:46:51 +01:00
parent 36fd3a9bfb
commit 2be1f473cd
5 changed files with 2616 additions and 256 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
config.h
View File

@ -34,8 +34,8 @@
#define Y_STEPS_PER_INCH Y_STEPS_PER_MM*INCHES_PER_MM #define Y_STEPS_PER_INCH Y_STEPS_PER_MM*INCHES_PER_MM
#define Z_STEPS_PER_INCH Z_STEPS_PER_MM*INCHES_PER_MM #define Z_STEPS_PER_INCH Z_STEPS_PER_MM*INCHES_PER_MM
#define RAPID_FEEDRATE 960.0 // in millimeters per minute #define RAPID_FEEDRATE 480.0 // in millimeters per minute
#define DEFAULT_FEEDRATE 960.0 #define DEFAULT_FEEDRATE 480.0
#define STEPPERS_ENABLE_DDR DDRD #define STEPPERS_ENABLE_DDR DDRD
#define STEPPERS_ENABLE_PORT PORTD #define STEPPERS_ENABLE_PORT PORTD

2518
gcode/braid_cut2d.gcode Normal file
View File

File diff suppressed because it is too large Load Diff

186
motion_control.c
View File

@ -81,13 +81,7 @@ void compute_and_set_step_pace(double feed_rate, double millimeters_of_travel, u
void mc_line(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)
{ {
// Flags to keep track of which axes to step // Flags to keep track of which axes to step
uint8_t step_bits; int32_t target[3]; // The target position in absolute steps
uint8_t axis; // loop variable
int8_t direction[3]; // The direction of travel along each axis (-1, 0 or 1)
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
// Setup --------------------------------------------------------------------------------------------------- // Setup ---------------------------------------------------------------------------------------------------
PORTD |= (1<<4); PORTD |= (1<<4);
@ -164,184 +158,6 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, 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) int axis_linear, double feed_rate, int invert_feed_rate)
{ {
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
int32_t x, y, target_x, target_y; // current position and target position in the
// local coordinate system of the arc-generator where [0,0] is the
// 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),
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
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
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.
error = x*x + y*y - radius_steps*radius_steps;
// Estimate length of arc in steps -------------------------------------------------------------------------
/*
To support helical motion we need to know in advance how many steppings the arc will need.
The calculations are based on the fact that we trace the circle by offsetting a square. The circle has
four "sides" or quadrants. For each quadrant we step mainly in one axis. The amount steps for one quarter of the
circle (e.g. along the x axis with positive y) is equal to one side of a square inscribed in the circle we
are tracing.
Quadrants of the circle
+---- 0 ----+ 0 - y is always positive and |x| < |y|
| | 1 - x is always positive and |x| > |y|
| | 2 - y is always negative and |x| < |y|
3 + 1 3 - x is always negative and |x| > |y|
| |
| | length of one side: 2*radius/sqrt(2)
+---- 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 arc_steps=0;
// Will this whole arc take place within the same quadrant?
if (start_quadrant == target_quadrant && (fabs(angular_travel) <= (M_PI/2))) {
if(quadrant_horizontal(start_quadrant)) { // a horizontal quadrant where x will be the primary direction
arc_steps = labs(target_x-start_x);
} else { // a vertical quadrant where y will be the primary direction
arc_steps = labs(target_y-start_y);
}
} else { // the start and target points are in different quadrants
// 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
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"
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.
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 = labs(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 2D linear bresenham as if the motion along the arc itself was a single axis
// of the line, while the linear "depth" axis was the other.
int32_t linear_counter = -maximum_steps/2;
int32_t arc_counter = -maximum_steps/2;
// Calculate feed rate -------------------------------------------------------------------------------------
// We then calculate the millimeters of helical travel
double millimeters_of_travel = hypot(angular_travel*radius, labs(linear_travel));
// 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.
compute_and_set_step_pace(feed_rate, millimeters_of_travel, maximum_steps, invert_feed_rate);
// Execution -----------------------------------------------------------------------------------------------
mode = MC_MODE_ARC;
// Set the direction of the linear or "depth" axis, cause it will never change
direction[axis_linear] = linear_direction;
// Cache some stepper bit-masks to speed up the interpolation code
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);
uint8_t step_bits;
while(mode)
{
// This loop sets the bits in the step_bits variable for each stepper it wants to step in this cycle.
step_bits = 0;
// The bresenham algorithm chooses when to travel in the depth axis and when to travel along the arc
linear_counter += linear_steps;
if (linear_counter > 0) {
linear_counter -= maximum_steps;
// Move one step in the depth direction:
step_bits |= axis_linear_bit;
}
arc_counter += arc_steps;
if (arc_counter > 0) {
arc_counter -= maximum_steps;
// Do one step of the arc:
// 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 (labs(x)<labs(y)) {
// X is major: Step arc horizontally
error += 1 + 2*x * dx;
x+=dx;
diagonal_error = error + 1 + 2*y*dy;
if(labs(error) >= labs(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(labs(error) >= labs(diagonal_error)) {
x += dx;
error = diagonal_error;
step_bits |= diagonal_bits; // step diagonal
} else {
step_bits |= axis_2_bit; // step straight
}
}
}
// Tell the steppers to do the stepping
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)))
{ mode = MC_MODE_AT_REST; } }
}
// Update the tool position to the new actual position
position[axis_1] += x-start_x;
position[axis_2] += y-start_y;
position[axis_2] += linear_steps*linear_direction;
} }
void mc_go_home() void mc_go_home()

144
stepper.c
View File

@ -32,47 +32,109 @@
#include "wiring_serial.h" #include "wiring_serial.h"
#define TICKS_PER_MICROSECOND (F_CPU/1000000) #define TICKS_PER_MICROSECOND (F_CPU/1000000)
#define STEP_BUFFER_SIZE 100 #define LINE_BUFFER_SIZE 5
// A marker used to notify the stepper handler of a pace change struct Line {
#define PACE_CHANGE_MARKER 0xff uint32_t steps_x, steps_y, steps_z;
uint32_t maximum_steps;
uint32_t iterations;
uint8_t direction_bits;
uint32_t rate;
}
volatile uint8_t step_buffer[STEP_BUFFER_SIZE]; // A buffer for step instructions volatile uint8_t line_buffer[LINE_BUFFER_SIZE]; // A buffer for step instructions
volatile int step_buffer_head = 0; volatile int line_buffer_head = 0;
volatile int step_buffer_tail = 0; volatile int line_buffer_tail = 0;
volatile uint32_t current_pace;
volatile uint32_t next_pace = 0; // Variables used by SIG_OUTPUT_COMPARE1A
uint8_t out_bits;
struct Line *current_line;
uint32_t counter_x, counter_y, counter_z;
uint8_t stepper_mode = STEPPER_MODE_STOPPED; uint8_t stepper_mode = STEPPER_MODE_STOPPED;
void config_pace_timer(uint32_t microseconds); void config_pace_timer(uint32_t microseconds);
void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t rate) {
// Buffer nothing unless stepping subsystem is running
if (stepper_mode != STEPPER_MODE_RUNNING) { return; }
// Calculate the buffer head after we push this byte
int next_buffer_head = (line_buffer_head + 1) % LINE_BUFFER_SIZE;
// If the buffer is full: good! That means we are well ahead of the robot.
// Nap until there is room in the buffer.
while(line_buffer_tail == next_buffer_head) { sleep_mode(); }
// setup line
struct Line *line = &line_buffer[line_buffer_head];
line->steps_x = labs(steps_x);
line->steps_y = labs(steps_y);
line->steps_z = labs(steps_y);
line->maximum_steps = max(line->steps_x, max(line->steps_y, line->steps_z));
line->iterations = line->maximum_steps;
line->rate = rate;
uint8_t direction_bits = 0;
if (steps_x < 0) { direction_bits |= (1<<X_DIRECTION_BIT); }
if (steps_y < 0) { direction_bits |= (1<<Y_DIRECTION_BIT); }
if (steps_z < 0) { direction_bits |= (1<<Z_DIRECTION_BIT); }
line->direction_bits = direction_bits;
// Move buffer head
line_buffer_head = next_buffer_head;
}
// This timer interrupt is executed at the pace set with st_buffer_pace. It pops one instruction from // This timer interrupt is executed at the pace set with st_buffer_pace. It pops one instruction from
// the step_buffer, executes it. Then it starts timer2 in order to reset the motor port after // the line_buffer, executes it. Then it starts timer2 in order to reset the motor port after
// five microseconds. // five microseconds.
SIGNAL(SIG_OUTPUT_COMPARE1A) SIGNAL(SIG_OUTPUT_COMPARE1A)
{ {
if (step_buffer_head != step_buffer_tail) {
PORTD &= ~(1<<3);
uint8_t popped = step_buffer[step_buffer_tail];
if(popped == PACE_CHANGE_MARKER) {
// This is not a step-instruction, but a pace-change-marker: change pace
config_pace_timer(next_pace);
next_pace = 0;
} else {
popped ^= STEPPING_INVERT_MASK;
// Set the direction pins a cuple of nanoseconds before we step the steppers // Set the direction pins a cuple of nanoseconds before we step the steppers
STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (popped & DIRECTION_MASK); STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
// Then pulse the stepping pins // Then pulse the stepping pins
STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | popped; STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | out_bits;
// Reset step pulse reset timer // Reset step pulse reset timer
TCNT2 = -(((STEP_PULSE_MICROSECONDS-4)*TICKS_PER_MICROSECOND)/8); TCNT2 = -(((STEP_PULSE_MICROSECONDS-4)*TICKS_PER_MICROSECOND)/8);
// If there is no current line, attempt to pop one from the buffer
if (current_line == NULL) {
// Anything in the buffer?
if (line_buffer_head != line_buffer_tail) {
// Retrieve a new line and get ready to step it
current_line = &line_buffer[line_buffer_tail];
config_pace_timer(current_line->rate);
counter_x = -current_line->maximum_steps/2;
counter_y = counter_x;
counter_z = counter_x;
// move the line buffer tail to the next instruction
line_buffer_tail = (line_buffer_tail + 1) % LINE_BUFFER_SIZE;
}
}
if (current_line != NULL) {
out_bits = current_line->direction_bits;
counter_x += current_line->steps_x;
if (counter_x > 0) {
out_bits |= (1<<X_STEP_BIT);
counter_x -= current_line->maximum_steps;
}
counter_y += current_line-> steps_y;
if (counter_y > 0) {
out_bits |= (1<<Y_STEP_BIT);
counter_y -= current_line->maximum_steps;
}
counter_z += current_line-> steps_z;
if (counter_z > 0) {
out_bits |= (1<<Z_STEP_BIT);
counter_z -= current_line->maximum_steps;
}
// If current line is finished, reset pointer
current_line->iterations -= 1;
if (current_line->iterations <= 0) {
current_line = NULL;
} }
// move the step buffer tail to the next instruction
step_buffer_tail = (step_buffer_tail + 1) % STEP_BUFFER_SIZE;
} else { } else {
PORTD |= (1<<3); out_bits = 0;
} }
out_bits ^= STEPPING_INVERT_MASK;
} }
// This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets // This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets
@ -113,25 +175,11 @@ void st_init()
config_pace_timer(20000); config_pace_timer(20000);
} }
inline void st_buffer_step(uint8_t motor_port_bits)
{
// Buffer nothing unless stepping subsystem is running
if (stepper_mode != STEPPER_MODE_RUNNING) { return; }
// Calculate the buffer head after we push this byte
int next_buffer_head = (step_buffer_head + 1) % STEP_BUFFER_SIZE;
// If the buffer is full: good! That means we are well ahead of the robot.
// Nap until there is room for more steps.
while(step_buffer_tail == next_buffer_head) { sleep_mode(); }
// Push byte
step_buffer[step_buffer_head] = motor_port_bits;
step_buffer_head = next_buffer_head;
}
// Block until all buffered steps are executed // Block until all buffered steps are executed
void st_synchronize() void st_synchronize()
{ {
if (stepper_mode == STEPPER_MODE_RUNNING) { if (stepper_mode == STEPPER_MODE_RUNNING) {
while(step_buffer_tail != step_buffer_head) { sleep_mode(); } while(line_buffer_tail != line_buffer_head) { sleep_mode(); }
} else { } else {
st_flush(); st_flush();
} }
@ -141,7 +189,8 @@ void st_synchronize()
void st_flush() void st_flush()
{ {
cli(); cli();
step_buffer_tail = step_buffer_head; line_buffer_tail = line_buffer_head;
current_line = NULL;
sei(); sei();
} }
@ -169,23 +218,6 @@ inline void st_stop()
stepper_mode = STEPPER_MODE_STOPPED; stepper_mode = STEPPER_MODE_STOPPED;
} }
// Buffer a pace change. Pace is the rate with which steps are executed. It is measured in microseconds from step to step.
// It is continually adjusted to achieve constant actual feed rate. Unless pace-changes was buffered along with the steps
// they govern they might change at slightly wrong moments in time as the pace would change while the stepper buffer was
// still churning out the previous movement.
void st_buffer_pace(uint32_t microseconds)
{
// Do nothing if the pace in unchanged or the stepping subsytem is not running
if ((current_pace == microseconds) || (stepper_mode != STEPPER_MODE_RUNNING)) { return; }
// If the single-element pace "buffer" is full, sleep until it is popped
while (next_pace != 0) {
sleep_mode();
}
// Buffer the pace change
next_pace = microseconds;
st_buffer_step(PACE_CHANGE_MARKER); // Place a pace-change marker in the step-buffer
}
// Returns a bitmask with the stepper bit for the given axis set // Returns a bitmask with the stepper bit for the given axis set
uint8_t st_bit_for_stepper(int axis) { uint8_t st_bit_for_stepper(int axis) {
switch(axis) { switch(axis) {

10
stepper.h
View File

@ -35,14 +35,8 @@ void st_init();
// Returns a bitmask with the stepper bit for the given axis set // Returns a bitmask with the stepper bit for the given axis set
uint8_t st_bit_for_stepper(int axis); uint8_t st_bit_for_stepper(int axis);
// Buffer a pace change. Pace is the rate with which steps are executed. It is measured in microseconds from step to step. // Buffer a new line segment (might block until there is room in the buffer)
// It is continually adjusted to achieve constant actual feed rate. Unless pace-changes was buffered along with the steps void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t rate);
// they govern they might change at slightly wrong moments in time as the pace would change while the stepper buffer was
// still churning out the previous movement.
void st_buffer_pace(uint32_t microseconds);
// Buffer a new instruction for the steppers
inline void st_buffer_step(uint8_t motor_port_bits);
// Block until all buffered steps are executed // Block until all buffered steps are executed
void st_synchronize(); void st_synchronize();