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

Refactored line buffering to eliminate state from motion control and centralize tracking of position. UNTESTED: NEEDS TESTING

Browse Source
This commit is contained in:
Simen Svale Skogsrud 2011-02-06 23:23:34 +01:00
parent cdcc7bf86e
commit c42741032f
7 changed files with 65 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
main.c
View File

@ -37,7 +37,6 @@ int main(void)
settings_init();
plan_init(); // initialize the stepper plan subsystem
st_init(); // initialize the stepper subsystem
mc_init(); // initialize motion control subsystem
spindle_init(); // initialize spindle controller
gc_init(); // initialize gcode-parser

36
motion_control.c
View File

@ -29,13 +29,6 @@
#include "stepper_plan.h"
#include "wiring_serial.h"
// The current position of the tool in absolute steps
int32_t position[3];
void mc_init()
{
clear_vector(position);
}
void mc_dwell(uint32_t milliseconds)
{
@ -48,31 +41,7 @@ void mc_dwell(uint32_t milliseconds)
// 1/feed_rate minutes.
void mc_line(double x, double y, double z, double feed_rate, int invert_feed_rate)
{
uint8_t axis; // loop variable
int32_t target[3]; // The target position in absolute steps
int32_t steps[3]; // The target line in relative steps
target[X_AXIS] = lround(x*settings.steps_per_mm[0]);
target[Y_AXIS] = lround(y*settings.steps_per_mm[1]);
target[Z_AXIS] = lround(z*settings.steps_per_mm[2]);
for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
steps[axis] = target[axis]-position[axis];
}
// Ask old Phytagoras to estimate how many mm our next move is going to take us
double millimeters_of_travel = sqrt(
square(steps[X_AXIS]/settings.steps_per_mm[0]) +
square(steps[Y_AXIS]/settings.steps_per_mm[1]) +
square(steps[Z_AXIS]/settings.steps_per_mm[2]));
if (invert_feed_rate) {
st_buffer_line(steps[X_AXIS], steps[Y_AXIS], steps[Z_AXIS], lround(ONE_MINUTE_OF_MICROSECONDS/feed_rate),
millimeters_of_travel);
} else {
st_buffer_line(steps[X_AXIS], steps[Y_AXIS], steps[Z_AXIS],
lround((millimeters_of_travel/feed_rate)*1000000), millimeters_of_travel);
}
memcpy(position, target, sizeof(target)); // position[] = target[]
st_buffer_line(x, y, z, feed_rate, invert_feed_rate);
}
// Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc,
@ -85,6 +54,8 @@ void mc_line(double x, double y, double z, double feed_rate, int invert_feed_rat
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)
{
int32_t position[3];
st_get_position_steps(&position);
int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
plan_set_acceleration_manager_enabled(FALSE); // disable acceleration management for the duration of the arc
double millimeters_of_travel = hypot(angular_travel*radius, labs(linear_travel));
@ -119,5 +90,4 @@ void mc_arc(double theta, double angular_travel, double radius, double linear_tr
void mc_go_home()
{
st_go_home();
clear_vector(position); // By definition this is location [0, 0, 0]
}

4
motion_control.h
View File

@ -23,10 +23,6 @@
#include <avr/io.h>
// Initializes the motion_control subsystem resources
void mc_init();
// 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 minute)/feed_rate time.

8
stepper.c
View File

@ -110,12 +110,16 @@ inline void trapezoid_generator_tick() {
// Add a new linear movement to the buffer. steps_x, _y and _z is the signed, relative motion in
// steps. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters) {
plan_buffer_line(steps_x, steps_y, steps_z, microseconds, millimeters);
void st_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate) {
plan_buffer_line(x, y, z, feed_rate, invert_feed_rate);
// Ensure that block processing is running by enabling The Stepper Driver Interrupt
ENABLE_STEPPER_DRIVER_INTERRUPT();
}
void st_get_position_steps(int32_t (*vector)[3]) {
memcpy(vector, position, sizeof(position)); // vector[] = position[]
}
// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is executed at the rate set with
// config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
// It is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse.

5
stepper.h
View File

@ -29,7 +29,10 @@ void st_init();
// Add a new linear movement to the buffer. steps_x, _y and _z is the signed, relative motion in
// steps. Microseconds specify how many microseconds the move should take to perform.
void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t rate, double millimeters);
void st_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate);
// Copy the current absolute position in steps into the provided vector
void st_get_position_steps(int32_t (*vector)[3]);
// Block until all buffered steps are executed
void st_synchronize();

70
stepper_plan.c
View File

@ -65,6 +65,9 @@ block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructio
volatile int block_buffer_head; // Index of the next block to be pushed
volatile int block_buffer_tail; // Index of the block to process now
// The current position of the tool in absolute steps
int32_t position[3];
static uint8_t acceleration_manager_enabled; // Acceleration management active?
// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the
@ -334,6 +337,7 @@ void plan_init() {
block_buffer_head = 0;
block_buffer_tail = 0;
plan_set_acceleration_manager_enabled(TRUE);
clear_vector(position);
}
void plan_set_acceleration_manager_enabled(int enabled) {
@ -347,10 +351,18 @@ int plan_is_acceleration_manager_enabled() {
return(acceleration_manager_enabled);
}
// Add a new linear movement to the buffer. steps_x, _y and _z is the signed, relative motion in
// steps. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters) {
void plan_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate) {
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
int32_t target[3];
target[0] = lround(x*settings.steps_per_mm[0]);
target[1] = lround(y*settings.steps_per_mm[1]);
target[2] = lround(y*settings.steps_per_mm[2]);
// Calculate the buffer head after we push this byte
int next_buffer_head = (block_buffer_head + 1) % BLOCK_BUFFER_SIZE;
// If the buffer is full: good! That means we are well ahead of the robot.
@ -359,25 +371,37 @@ void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Number of steps for each axis
block->steps_x = labs(steps_x);
block->steps_y = labs(steps_y);
block->steps_z = labs(steps_z);
block->steps_x = labs(position[0]-target[0]);
block->steps_y = labs(position[1]-target[1]);
block->steps_z = labs(position[2]-target[2]);
block->millimeters = sqrt(
square(block->steps_x/settings.steps_per_mm[0])+
square(block->steps_y/settings.steps_per_mm[1])+
square(block->steps_z/settings.steps_per_mm[2]));
block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z));
// Bail if this is a zero-length block
if (block->step_event_count == 0) { return; };
uint32_t microseconds;
if (!invert_feed_rate) {
microseconds = lround((block->millimeters/feed_rate)*1000000);
} else {
microseconds = lround(ONE_MINUTE_OF_MICROSECONDS/feed_rate);
}
// Calculate speed in mm/minute for each axis
double multiplier = 60.0*1000000.0/microseconds;
// printInteger(multiplier*1000); printString("<-multi\n\r");
block->speed_x = steps_x*multiplier/settings.steps_per_mm[0];
block->speed_y = steps_y*multiplier/settings.steps_per_mm[1];
block->speed_z = steps_z*multiplier/settings.steps_per_mm[2];
block->nominal_speed = millimeters*multiplier;
block->speed_x = x*multiplier;
block->speed_y = y*multiplier;
block->speed_z = z*multiplier;
block->nominal_speed = block->millimeters*multiplier;
// printInteger(millimeters*1000); printString("<-mm\n\r");
// printInteger(block->nominal_speed*1000); printString("<-ns\n\r");
block->nominal_rate = ceil(block->step_event_count*multiplier);
// printInteger(block->nominal_rate*1000); printString("<-nr\n\r");
// printInteger((uint16_t)block); printString("<-addr\n\r");
block->millimeters = millimeters;
block->entry_factor = 0.0;
// Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
@ -386,13 +410,14 @@ void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_
// axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
// To generate trapezoids with contant acceleration between blocks the rate_delta must be computed
// specifically for each line to compensate for this phenomenon:
double travel_per_step = millimeters/block->step_event_count;
double travel_per_step = block->millimeters/block->step_event_count;
block->rate_delta = ceil(
((settings.acceleration*60.0)/(ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/sec/sec per acceleration_tick
travel_per_step); // convert to: acceleration steps/min/acceleration_tick
if (acceleration_manager_enabled) {
// compute a preliminary conservative acceleration trapezoid
double safe_speed_factor = factor_for_safe_speed(block);
calculate_trapezoid_for_block(block, safe_speed_factor, safe_speed_factor); // compute a conservative acceleration trapezoid for now
calculate_trapezoid_for_block(block, safe_speed_factor, safe_speed_factor);
} else {
block->initial_rate = block->nominal_rate;
block->accelerate_until = 0;
@ -402,16 +427,15 @@ void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_
// Compute direction bits for this block
block->direction_bits = 0;
if (steps_x < 0) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
if (steps_y < 0) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
if (steps_z < 0) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
// Move buffer head
block_buffer_head = next_buffer_head;
if (target[0] < position[0]) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
if (target[1] < position[1]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
if (target[2] < position[2]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
if (acceleration_manager_enabled) {
planner_recalculate();
} else {
calculate_trapezoid_for_block(block, 1.0, 1.0);
}
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
memcpy(position, target, sizeof(target)); // position[] = target[]
if (acceleration_manager_enabled) { planner_recalculate(); }
}

6
stepper_plan.h
View File

@ -18,6 +18,9 @@
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
// This module is to be considered a sub-module of stepper.c. Please don't include
// this file from any other module.
#ifndef stepper_plan_h
#define stepper_plan_h
@ -56,6 +59,7 @@ typedef struct {
extern block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions
extern volatile int block_buffer_head; // Index of the next block to be pushed
extern volatile int block_buffer_tail; // Index of the block to process now
extern int32_t position[3];
// Initialize the motion plan subsystem
void plan_init();
@ -65,7 +69,7 @@ void plan_init();
// Add a new linear movement to the buffer. steps_x, _y and _z is the signed, relative motion in
// steps. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters);
void plan_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate);
// Enables acceleration-management for upcoming blocks
void plan_set_acceleration_manager_enabled(int enabled);