From c42741032fe699c3cd37f4e5d8dd91dc9c5a20aa Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sun, 6 Feb 2011 23:23:34 +0100 Subject: [PATCH] Refactored line buffering to eliminate state from motion control and centralize tracking of position. UNTESTED: NEEDS TESTING --- main.c | 1 - motion_control.c | 36 +++---------------------- motion_control.h | 4 --- stepper.c | 8 ++++-- stepper.h | 5 +++- stepper_plan.c | 70 ++++++++++++++++++++++++++++++++---------------- stepper_plan.h | 6 ++++- 7 files changed, 65 insertions(+), 65 deletions(-) diff --git a/main.c b/main.c index 5b050a7..9fcd889 100644 --- a/main.c +++ b/main.c @@ -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 diff --git a/motion_control.c b/motion_control.c index ba15dfb..9ddfb2d 100644 --- a/motion_control.c +++ b/motion_control.c @@ -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] } diff --git a/motion_control.h b/motion_control.h index 13fc89d..2f1737e 100644 --- a/motion_control.h +++ b/motion_control.h @@ -23,10 +23,6 @@ #include - -// 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. diff --git a/stepper.c b/stepper.c index f8e73f3..dba9593 100644 --- a/stepper.c +++ b/stepper.c @@ -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. diff --git a/stepper.h b/stepper.h index e667c91..e51e2a2 100644 --- a/stepper.h +++ b/stepper.h @@ -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(); diff --git a/stepper_plan.c b/stepper_plan.c index d1beb65..9a7d3bd 100644 --- a/stepper_plan.c +++ b/stepper_plan.c @@ -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<direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<. */ +// 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);