From a1397f61c4862f95f85f12e5facc103b0d836f54 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sun, 16 Dec 2012 16:23:24 -0700 Subject: [PATCH] Max velocity axes independence installed. Fixed intermittent slow trailing steps. Timer0 disable fix. - Maximum velocity for each axis is now configurable in settings. All rapids/seek move at these maximums. All feed rates(including rapids) may be limited and scaled down so that no axis does not exceed their limits. - Moved around auto-cycle start. May change later, but mainly to ensure the planner buffer is completely full before cycle starting a streaming program. Otherwise it should auto-start when there is a break in the serial stream. - Reverted old block->max_entry_speed_sqr calculations. Feedrate overrides not close to ready at all. - Fixed intermittent slow trailing steps for some triangle velocity profile moves. The acceleration tick counter updating was corrected to be exact for that particular transition. Should be ok for normal trapezoidal profiles. - Fixed the Timer0 disable after a step pulse falling edge. Thanks @blinkenlight! --- config.h | 2 +- gcode.c | 17 ++--- gcode.h | 1 - main.c | 5 ++ motion_control.c | 6 +- planner.c | 189 ++++++++++++++++++++++++++--------------------- report.c | 46 ++++++------ settings.c | 51 +++++++------ settings.h | 4 +- stepper.c | 27 ++++--- 10 files changed, 188 insertions(+), 160 deletions(-) diff --git a/config.h b/config.h index b164648..a43f15c 100644 --- a/config.h +++ b/config.h @@ -122,7 +122,7 @@ // frequency goes up. So there will be little left for other processes like arcs. // In future versions, more work will be done to increase the step rates but still stay around // 20kHz by performing two steps per step event, rather than just one. -#define ISR_TICKS_PER_SECOND 30000L // Integer (Hz) +#define ISR_TICKS_PER_SECOND 20000L // Integer (Hz) // The Ranade algorithm can use either floating point or long integers for its counters, but for // integers the counter values must be scaled since these values can be very small (10^-6). This diff --git a/gcode.c b/gcode.c index c741fc2..5d3404f 100644 --- a/gcode.c +++ b/gcode.c @@ -51,8 +51,7 @@ static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) void gc_init() { memset(&gc, 0, sizeof(gc)); - gc.feed_rate = settings.default_feed_rate; // Should be zero at initialization. -// gc.seek_rate = settings.default_seek_rate; + gc.feed_rate = settings.default_feed_rate; select_plane(X_AXIS, Y_AXIS, Z_AXIS); gc.absolute_mode = true; @@ -247,9 +246,7 @@ uint8_t gc_execute_line(char *line) NOTE: Independent non-motion/settings parameters are set out of this order for code efficiency and simplicity purposes, but this should not affect proper g-code execution. */ - // ([F]: Set feed and seek rates.) - // TODO: Seek rates can change depending on the direction and maximum speeds of each axes. When - // max axis speed is installed, the calculation can be performed here, or maybe in the planner. + // ([F]: Set feed rate.) if (sys.state != STATE_CHECK_MODE) { // ([M6]: Tool change should be executed here.) @@ -324,14 +321,14 @@ uint8_t gc_execute_line(char *line) target[i] = gc.position[i]; } } - mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], settings.default_seek_rate, false); + mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], -1.0, false); } // Retreive G28/30 go-home position data (in machine coordinates) from EEPROM float coord_data[N_AXIS]; uint8_t home_select = SETTING_INDEX_G28; if (non_modal_action == NON_MODAL_GO_HOME_1) { home_select = SETTING_INDEX_G30; } if (!settings_read_coord_data(home_select,coord_data)) { return(STATUS_SETTING_READ_FAIL); } - mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], settings.default_seek_rate, false); + mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], -1.0, false); memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[]; axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags. break; @@ -347,7 +344,7 @@ uint8_t gc_execute_line(char *line) // Update axes defined only in block. Offsets current system to defined value. Does not update when // active coordinate system is selected, but is still active unless G92.1 disables it. uint8_t i; - for (i=0; i<=2; i++) { // Axes indices are consistent, so loop may be used. + for (i=0; iinitial_rate = ceil(sqrt(entry_speed_sqr)*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - + // TODO: Compute new nominal rate if a feedrate override occurs. // block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - // Compute efficiency variable for following calculations. Removes a float divide and multiply. // TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds. @@ -155,11 +164,29 @@ static void calculate_trapezoid_for_block(block_t *block, float entry_speed_sqr, All planner computations(1)(2) are performed in floating point to minimize numerical round-off errors. Only when planned values are converted to stepper rate parameters(3), these are integers. If another motion block is added while executing, the planner will re-plan and update the stored optimal velocity profile as it goes. + + Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's + constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and + nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile + through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting + conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile. + + Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line + segments, like arcs, may seem to move slow. This is because there simply isn't enough combined distance traveled + in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the + buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be + able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance. + The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED. + Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost + steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the + bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory + is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering + up to a hundred or more. - NOTE: As executing blocks complete and incoming streaming blocks are appended to the planner buffer, this - function is constantly re-calculating and must be as efficient as possible. For example, in situations like - arc generation or complex curves, the short, rapid line segments can execute faster than new blocks can be - added, and the planner buffer will then starve and empty, leading to weird hiccup-like jerky motions. + NOTE: Since this function is constantly re-calculating for every new incoming block, it must be as efficient + as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments + can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading + to weird hiccup-like jerky motions. */ static void planner_recalculate() { @@ -170,7 +197,6 @@ static void planner_recalculate() // if (block_buffer_head != block_buffer_tail) { float entry_speed_sqr; - float max_entry_speed_sqr; // Perform reverse planner pass. Skip the head(end) block since it is already initialized, and skip the // tail(first) block to prevent over-writing of the initial entry speed. @@ -182,24 +208,23 @@ static void planner_recalculate() next = current; current = &block_buffer[block_index]; - // Determine maximum entry speed at junction. Computed here since feedrate overrides can alter - // the planner nominal speeds at any time. - max_entry_speed_sqr = current->max_entry_speed_sqr; - if (max_entry_speed_sqr > current->nominal_speed_sqr) { max_entry_speed_sqr = current->nominal_speed_sqr; } - if (max_entry_speed_sqr > next->nominal_speed_sqr) { max_entry_speed_sqr = next->nominal_speed_sqr; } + // TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter + // the planner nominal speeds at any time. This calc could be done in the override handler, but + // this could require an additional variable to be stored to differentiate the programmed nominal + // speeds, max junction speed, and override speeds/scalar. // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and // check for maximum allowable speed reductions to ensure maximum possible planned speed. - if (current->entry_speed_sqr != max_entry_speed_sqr) { + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { - current->entry_speed_sqr = max_entry_speed_sqr; + current->entry_speed_sqr = current->max_entry_speed_sqr; current->recalculate_flag = true; // Almost always changes. So force recalculate. - if (max_entry_speed_sqr > next->entry_speed_sqr) { + if (next->entry_speed_sqr < current->max_entry_speed_sqr) { // Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; - if (entry_speed_sqr < max_entry_speed_sqr) { + if (entry_speed_sqr < current->max_entry_speed_sqr) { current->entry_speed_sqr = entry_speed_sqr; } } @@ -253,15 +278,6 @@ void plan_reset_buffer() next_buffer_head = next_block_index(block_buffer_head); } -void plan_init() -{ - plan_reset_buffer(); - memset(&pl, 0, sizeof(pl)); // Clear planner struct - pl.inverse_steps_per_mm[X_AXIS] = 1/settings.steps_per_mm[X_AXIS]; - pl.inverse_steps_per_mm[Y_AXIS] = 1/settings.steps_per_mm[Y_AXIS]; - pl.inverse_steps_per_mm[Z_AXIS] = 1/settings.steps_per_mm[Z_AXIS]; -} - inline void plan_discard_current_block() { if (block_buffer_head != block_buffer_tail) { @@ -278,7 +294,11 @@ inline block_t *plan_get_current_block() // Returns the availability status of the block ring buffer. True, if full. uint8_t plan_check_full_buffer() { - if (block_buffer_tail == next_buffer_head) { return(true); } + if (block_buffer_tail == next_buffer_head) { + // TODO: Move this back into motion control. Shouldn't be here, but it's efficient. + if (sys.auto_start) { st_cycle_start(); } // Auto-cycle start when buffer is full. + return(true); + } return(false); } @@ -298,10 +318,11 @@ void plan_synchronize() // All position data passed to the planner must be in terms of machine position to keep the planner // independent of any coordinate system changes and offsets, which are handled by the g-code parser. // NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. +// Also the feed rate input value is used in three ways: as a normal feed rate if invert_feed_rate +// is false, as inverse time if invert_feed_rate is true, or as seek/rapids rate if the feed_rate +// value is negative (and invert_feed_rate always false). void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate) { -// SPINDLE_ENABLE_PORT ^= 1<step_event_count == 0) { return; }; // Compute path vector in terms of absolute step target and current positions - // TODO: Store last xyz in memory to remove steps_per_mm divides. Or come up with another way to save cycles. - // Or store pl.inverse_steps_per_mm to speed this up. Would be computed at initialization. float delta_mm[3]; -// delta_mm[X_AXIS] = block->steps_x/settings.steps_per_mm[X_AXIS]; // 110 usec -// delta_mm[Y_AXIS] = block->steps_y/settings.steps_per_mm[Y_AXIS]; -// delta_mm[Z_AXIS] = block->steps_z/settings.steps_per_mm[Z_AXIS]; - delta_mm[X_AXIS] = block->steps_x*pl.inverse_steps_per_mm[X_AXIS]; // 50usec - delta_mm[Y_AXIS] = block->steps_y*pl.inverse_steps_per_mm[Y_AXIS]; - delta_mm[Z_AXIS] = block->steps_z*pl.inverse_steps_per_mm[Z_AXIS]; + delta_mm[X_AXIS] = x-pl.last_x; + delta_mm[Y_AXIS] = y-pl.last_y; + delta_mm[Z_AXIS] = z-pl.last_z; block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + delta_mm[Z_AXIS]*delta_mm[Z_AXIS]); - // Compute absolute value of the path unit vector for acceleration calculations. - float unit_vec[3]; - float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides - unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; - unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; - unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; + // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) + // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort. + if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later + else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } - // Calculate block acceleration based on the maximum possible axes acceleration parameters. + // Calculate the unit vector of the line move and the block maximum feed rate and acceleration limited + // by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so + // they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction + // and axes properties as well. // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. - // TODO: Install pl.inverse_acceleration to possibly speed up this calculation. - float accel_inv_scalar = 0.0; - if (unit_vec[X_AXIS]) { accel_inv_scalar = max(accel_inv_scalar,unit_vec[X_AXIS]/settings.acceleration[X_AXIS]); } - if (unit_vec[Y_AXIS]) { accel_inv_scalar = max(accel_inv_scalar,unit_vec[Y_AXIS]/settings.acceleration[Y_AXIS]); } - if (unit_vec[Z_AXIS]) { accel_inv_scalar = max(accel_inv_scalar,unit_vec[Z_AXIS]/settings.acceleration[Z_AXIS]); } - block->acceleration = 1.0/accel_inv_scalar; + uint8_t i; + float unit_vec[N_AXIS], inverse_unit_vec_value; + float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides + block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration + for (i=0; iacceleration = min(block->acceleration,settings.acceleration[i]*inverse_unit_vec_value); + } + } - // Compute direction bits and apply unit vector directions for junction speed calculations - block->direction_bits = 0; - if (target[X_AXIS] < pl.position[X_AXIS]) { - block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<millimeters/feed_rate; } + // Compute nominal speed and rates block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0 block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) - - // TODO: When acceleration independence is installed, it can be kept in terms of 2*acceleration for - // each block. This could save some 2*acceleration multiplications elsewhere. Need to check though. - + // Compute the acceleration and distance traveled per step event for the stepper algorithm. block->rate_delta = ceil(block->acceleration* - (RANADE_MULTIPLIER/(60.0*60.0*ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) + ((RANADE_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step) + + // Compute direction bits. Bit enabled always means direction is negative. + block->direction_bits = 0; + if (unit_vec[X_AXIS] < 0) { block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<max_entry_speed_sqr = min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr); // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. if (cos_theta > -0.95) { // Compute maximum junction velocity based on maximum acceleration and junction deviation float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. - block->max_entry_speed_sqr = block->acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2); - } else { - block->max_entry_speed_sqr = 1000000.0; //MAXIMUM_JUNCTION_SPEED; + block->max_entry_speed_sqr = min(block->max_entry_speed_sqr, + block->acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)); } } } // Initialize block entry speed. Compute block entry velocity backwards from user-defined MINIMUM_PLANNER_SPEED. - block->entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*block->acceleration*block->millimeters; - if (block->entry_speed_sqr > pl.previous_nominal_speed_sqr) { block->entry_speed_sqr = pl.previous_nominal_speed_sqr; } - if (block->entry_speed_sqr > block->nominal_speed_sqr) { block->entry_speed_sqr = block->nominal_speed_sqr; } - if (block->entry_speed_sqr > block->max_entry_speed_sqr) { block->entry_speed_sqr = block->max_entry_speed_sqr; } + // TODO: This could be moved to the planner recalculate function. + block->entry_speed_sqr = min( block->max_entry_speed_sqr, + MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*block->acceleration*block->millimeters); // Set new block to be recalculated for conversion to stepper data. block->recalculate_flag = true; @@ -429,24 +441,29 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert // Update previous path unit_vector and nominal speed (squared) memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; - + + // Update planner position + memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[] + pl.last_x = x; + pl.last_y = y; + pl.last_z = z; + // Update buffer head and next buffer head indices block_buffer_head = next_buffer_head; next_buffer_head = next_block_index(block_buffer_head); - - // Update planner position - memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[] planner_recalculate(); -// SPINDLE_ENABLE_PORT ^= 1<direction_bits ^ (settings.invert_mask & DIRECTION_MASK); - + out_bits = current_block->direction_bits ^ settings.invert_mask; + st.execute_step = true; // Set flag to set direction bits. + // Initialize Bresenham variables st.counter_x = (current_block->step_event_count >> 1); st.counter_y = st.counter_x; @@ -216,7 +215,7 @@ ISR(TIMER2_COMPA_vect) if (st.delta_d > current_block->rate_delta) { st.delta_d -= current_block->rate_delta; } else { - st.delta_d = MINIMUM_STEP_RATE; // Prevent integer overflow + st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow. } } } @@ -278,10 +277,14 @@ ISR(TIMER2_COMPA_vect) if (st.ramp_type != DECEL_RAMP) { // Acceleration and cruise handled by ramping. Just check for deceleration. if (st.step_events_remaining <= current_block->decelerate_after) { - if (st.step_events_remaining == current_block->decelerate_after) { - st.ramp_count = INTERRUPTS_PER_ACCELERATION_TICK/2; - } st.ramp_type = DECEL_RAMP; + if (st.step_events_remaining == current_block->decelerate_after) { + if (st.delta_d == current_block->nominal_rate) { + st.ramp_count = INTERRUPTS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid + } else { + st.ramp_count = INTERRUPTS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle + } + } } } } else { @@ -302,7 +305,7 @@ ISR(TIMER2_COMPA_vect) ISR(TIMER0_OVF_vect) { STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); - TCNT0 = 0; // Disable timer until needed. + TCCR0B = 0; // Disable timer until needed. }