From 9141ad282540eaa50a41283685f901f29c24ddbd Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Tue, 11 Oct 2011 20:51:04 -0600 Subject: [PATCH] Third time's a charm! No more deceleration issues! Updated grbl version and settings. General cleanup. - Fleshed out the original idea to completely remove the long slope at the end of deceleration issue. This third time should absolutely eliminate it. - Changed the acceleration setting to kept as mm/min^2 internally, since this was creating unneccessary additional computation in the planner. Human readable value kept at mm/sec^2. - Updated grbl version 0.7d and settings version to 4. NOTE: Please check settings after update. These may have changed, but shouldn't. - Before updating the new features (pause, e-stop, federate override, etc), the edge branch will soon be merged with the master, barring any immediate issues that people may have, and the edge branch will be the testing ground for the new grbl version 0.8. --- config.h | 2 +- gcode.c | 6 +- motion_control.c | 8 +- motion_control_new.c | 207 +++++++++++++++++++++++++++++++++++++++++++ planner.c | 12 +-- planner.h | 2 +- settings.c | 19 ++-- settings.h | 4 +- stepper.c | 56 ++++++++---- stepper.h | 5 +- 10 files changed, 270 insertions(+), 51 deletions(-) create mode 100644 motion_control_new.c diff --git a/config.h b/config.h index 17a566d..db4d361 100644 --- a/config.h +++ b/config.h @@ -123,4 +123,4 @@ // // #define SPINDLE_DIRECTION_DDR DDRD // #define SPINDLE_DIRECTION_PORT PORTD -// #define SPINDLE_DIRECTION_BIT 7 \ No newline at end of file +// #define SPINDLE_DIRECTION_BIT 7 diff --git a/gcode.c b/gcode.c index 1930389..4b50f06 100644 --- a/gcode.c +++ b/gcode.c @@ -116,9 +116,6 @@ uint8_t gc_execute_line(char *line) { double p = 0, r = 0; int int_value; - - clear_vector(target); - clear_vector(offset); gc.status_code = STATUS_OK; @@ -171,6 +168,7 @@ uint8_t gc_execute_line(char *line) { if (gc.status_code) { return(gc.status_code); } char_counter = 0; + clear_vector(target); clear_vector(offset); memcpy(target, gc.position, sizeof(target)); // i.e. target = gc.position @@ -388,4 +386,4 @@ static int next_statement(char *letter, double *double_ptr, char *line, uint8_t group 9 = {M48, M49} enable/disable feed and speed override switches group 12 = {G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3} coordinate system selection group 13 = {G61, G61.1, G64} path control mode -*/ \ No newline at end of file +*/ diff --git a/motion_control.c b/motion_control.c index 52a72c2..c7e5670 100644 --- a/motion_control.c +++ b/motion_control.c @@ -42,12 +42,6 @@ void mc_dwell(double seconds) } } -// Execute an arc in offset mode format. position == current xyz, target == target xyz, -// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is -// the direction of helical travel, radius == circle radius, isclockwise boolean. Used -// for vector transformation direction. -// position, target, and offset are pointers to vectors from gcode.c - #ifdef __AVR_ATmega328P__ // The arc is approximated by generating a huge number of tiny, linear segments. The length of each // segment is configured in settings.mm_per_arc_segment. @@ -155,4 +149,4 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui void mc_go_home() { st_go_home(); -} \ No newline at end of file +} diff --git a/motion_control_new.c b/motion_control_new.c new file mode 100644 index 0000000..6ca0c9f --- /dev/null +++ b/motion_control_new.c @@ -0,0 +1,207 @@ +/* + motion_control.c - high level interface for issuing motion commands + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + Copyright (c) 2011 Sungeun K. Jeon + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#include +#include "settings.h" +#include "config.h" +#include "motion_control.h" +#include +#include +#include +#include "nuts_bolts.h" +#include "stepper.h" +#include "planner.h" + +// Execute dwell in seconds. Maximum time delay is > 18 hours, more than enough for any application. +void mc_dwell(double seconds) +{ + uint16_t i = floor(seconds); + st_synchronize(); + _delay_ms(floor(1000*(seconds-i))); // Delay millisecond remainder + while (i > 0) { + _delay_ms(1000); // Delay one second + i--; + } +} + +// void mc_jog_enable() +// { +// // Planned sequence of events: +// // Send X,Y,Z motion, target step rate, direction +// // Rate_delta, step_xyz, counter_xyz should be all the same. +// // + +// Change of direction can cause some problems. Need to force a complete stop for any direction change. +// This likely needs to be done in stepper.c as a jog mode parameter. + +// !!! Need a way to get step locations realtime!!! +// Jog is a specialized case, where grbl is reset and there is no cycle start. +// If there is a real-time status elsewhere, this shouldn't be a problem. + +// st.direction_bits = current_block->direction_bits; +// st.target_rate; +// st.rate_delta; +// st.step_event_count; +// st.steps_x; +// st.steps_y; +// st.steps_z; +// st.counter_x = -(current_block->step_event_count >> 1); +// st.counter_y = st.counter_x; +// st.counter_z = st.counter_x; +// st.step_event_count = current_block->step_event_count; +// st.step_events_completed = 0; +// } + +// void mc_jog_disable() +// { +// // Calls stepper.c and disables jog mode to start deceleration. +// // Shouldn't have to anything else. Just initiate the stop, so if re-enabled, it can accelerate. +// } + +// void mc_feed_hold() +// { +// // Planned sequence of events: +// // Query stepper for interrupting cycle and hold until pause flag is set? +// // Query stepper intermittenly and check for !st.do_motion to indicate complete stop. +// // Retreive st.step_events_completed and recompute current location. +// // Truncate current block start to current location. +// // Re-plan buffer for start from zero velocity and truncated block length. +// // All necessary computations for a restart should be done by now. +// // Reset pause flag. +// // Only wait for a cycle start command from user interface. (TBD). +// // !!! Need to check how to circumvent the wait in the main program. May need to be in serial.c +// // as an interrupt process call. Can two interrupt programs exist at the same time?? +// } + +// Execute an arc in offset mode format. position == current xyz, target == target xyz, +// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is +// the direction of helical travel, radius == circle radius, isclockwise boolean. Used +// for vector transformation direction. +// position, target, and offset are pointers to vectors from gcode.c + +#ifdef __AVR_ATmega328P__ +// The arc is approximated by generating a huge number of tiny, linear segments. The length of each +// segment is configured in settings.mm_per_arc_segment. +void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1, + uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, uint8_t isclockwise) +{ +// 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 center_axis0 = position[axis_0] + offset[axis_0]; + double center_axis1 = position[axis_1] + offset[axis_1]; + double linear_travel = target[axis_linear] - position[axis_linear]; + double r_axis0 = -offset[axis_0]; // Radius vector from center to current location + double r_axis1 = -offset[axis_1]; + double rt_axis0 = target[axis_0] - center_axis0; + double rt_axis1 = target[axis_1] - center_axis1; + + // CCW angle between position and target from circle center. Only one atan2() trig computation required. + double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1); + if (angular_travel < 0) { angular_travel += 2*M_PI; } + if (isclockwise) { angular_travel -= 2*M_PI; } + + double millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel)); + if (millimeters_of_travel == 0.0) { return; } + uint16_t segments = floor(millimeters_of_travel/settings.mm_per_arc_segment); + // Multiply inverse feed_rate to compensate for the fact that this movement is approximated + // by a number of discrete segments. The inverse feed_rate should be correct for the sum of + // all segments. + if (invert_feed_rate) { feed_rate *= segments; } + + double theta_per_segment = angular_travel/segments; + double linear_per_segment = linear_travel/segments; + + /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector, + and phi is the angle of rotation. Based on the solution approach by Jens Geisler. + r_T = [cos(phi) -sin(phi); + sin(phi) cos(phi] * r ; + + For arc generation, the center of the circle is the axis of rotation and the radius vector is + defined from the circle center to the initial position. Each line segment is formed by successive + vector rotations. This requires only two cos() and sin() computations to form the rotation + matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since + all double numbers are single precision on the Arduino. (True double precision will not have + round off issues for CNC applications.) Single precision error can accumulate to be greater than + tool precision in some cases. Therefore, arc path correction is implemented. + + Small angle approximation may be used to reduce computation overhead further. This approximation + holds for everything, but very small circles and large mm_per_arc_segment values. In other words, + theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large + to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for + numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an + issue for CNC machines with the single precision Arduino calculations. + + This approximation also allows mc_arc to immediately insert a line segment into the planner + without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied + a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. + This is important when there are successive arc motions. + */ + // Vector rotation matrix values + double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation + double sin_T = theta_per_segment; + + double arc_target[3]; + double sin_Ti; + double cos_Ti; + double r_axisi; + uint16_t i; + int8_t count = 0; + + // Initialize the linear axis + arc_target[axis_linear] = position[axis_linear]; + + for (i = 1; iinitial_rate, block->nominal_rate, acceleration_per_minute)); int32_t decelerate_steps = floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration_per_minute)); - + // Calculate the size of Plateau of Nominal Rate. int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; @@ -382,7 +382,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in // specifically for each line to compensate for this phenomenon: // Convert universal acceleration for direction-dependent stepper rate change parameter block->rate_delta = ceil( block->step_event_count*inverse_millimeters * - settings.acceleration*60.0 / ACCELERATION_TICKS_PER_SECOND ); // (step/min/acceleration_tick) + settings.acceleration / (60 * ACCELERATION_TICKS_PER_SECOND )); // (step/min/acceleration_tick) // Perform planner-enabled calculations if (acceleration_manager_enabled) { @@ -421,7 +421,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in // Compute maximum junction velocity based on maximum acceleration and junction deviation double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. vmax_junction = min(vmax_junction, - sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); + sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); } } } @@ -462,7 +462,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in memcpy(position, target, sizeof(target)); // position[] = target[] if (acceleration_manager_enabled) { planner_recalculate(); } - st_wake_up(); + st_cycle_start(); } // Reset the planner position vector and planner speed @@ -472,4 +472,4 @@ void plan_set_current_position(double x, double y, double z) { position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. clear_vector_double(previous_unit_vec); -} \ No newline at end of file +} diff --git a/planner.h b/planner.h index f995210..f91b7c2 100644 --- a/planner.h +++ b/planner.h @@ -74,4 +74,4 @@ int plan_is_acceleration_manager_enabled(); // Reset the position vector void plan_set_current_position(double x, double y, double z); -#endif \ No newline at end of file +#endif diff --git a/settings.c b/settings.c index f85e3c3..09514d6 100644 --- a/settings.c +++ b/settings.c @@ -49,10 +49,10 @@ typedef struct { #define DEFAULT_Z_STEPS_PER_MM (94.488188976378*MICROSTEPS) #define DEFAULT_STEP_PULSE_MICROSECONDS 30 #define DEFAULT_MM_PER_ARC_SEGMENT 0.1 -#define DEFAULT_RAPID_FEEDRATE 500.0 // in millimeters per minute +#define DEFAULT_RAPID_FEEDRATE 500.0 // mm/min #define DEFAULT_FEEDRATE 500.0 -#define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/10.0) -#define DEFAULT_JUNCTION_DEVIATION 0.05 +#define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE*60*60/10.0) // mm/min^2 +#define DEFAULT_JUNCTION_DEVIATION 0.05 // mm #define DEFAULT_STEPPING_INVERT_MASK ((1< #include -#define GRBL_VERSION "0.7c" +#define GRBL_VERSION "0.7d" // Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl // when firmware is upgraded. Always stored in byte 0 of eeprom -#define SETTINGS_VERSION 3 +#define SETTINGS_VERSION 4 // Current global settings (persisted in EEPROM from byte 1 onwards) typedef struct { diff --git a/stepper.c b/stepper.c index a520038..c8ee56c 100644 --- a/stepper.c +++ b/stepper.c @@ -3,7 +3,7 @@ Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud - Modifications Copyright (c) 2011 Sungeun K. Jeon + Copyright (c) 2011 Sungeun K. Jeon Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -56,6 +56,8 @@ static uint32_t cycles_per_step_event; // The number of machine cycles be static uint32_t trapezoid_tick_cycle_counter; // The cycles since last trapezoid_tick. Used to generate ticks at a steady // pace without allocating a separate timer static uint32_t trapezoid_adjusted_rate; // The current rate of step_events according to the trapezoid generator +static uint32_t min_safe_rate; // Minimum safe rate for full deceleration rate reduction step. Otherwise halves step_rate. +static uint8_t cycle_start; // Cycle start flag to indicate program start and block processing. // __________________________ // /| |\ _________________ ^ @@ -76,14 +78,20 @@ static uint32_t trapezoid_adjusted_rate; // The current rate of step_events static void set_step_events_per_minute(uint32_t steps_per_minute); +// Stepper state initialization void st_wake_up() { + // Initialize stepper output bits + out_bits = (0) ^ (settings.invert_mask); // Enable steppers by resetting the stepper disable port STEPPERS_DISABLE_PORT &= ~(1<initial_rate; + trapezoid_adjusted_rate = current_block->initial_rate; + min_safe_rate = current_block->rate_delta + (current_block->rate_delta >> 1); // 1.5 x rate_delta trapezoid_tick_cycle_counter = CYCLES_PER_ACCELERATION_TICK/2; // Start halfway for midpoint rule. set_step_events_per_minute(trapezoid_adjusted_rate); // Initialize cycles_per_step_event } @@ -116,23 +125,24 @@ static uint8_t iterate_trapezoid_cycle_counter() { } } -// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is executed at the rate set with +// "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. // The bresenham line tracer algorithm controls all three stepper outputs simultaneously with these two interrupts. SIGNAL(TIMER1_COMPA_vect) { - // TODO: Check if the busy-flag can be eliminated by just disabeling this interrupt while we are in it + // TODO: Check if the busy-flag can be eliminated by just disabling this interrupt while we are in it if(busy){ return; } // The busy-flag is used to avoid reentering this interrupt - // Set the direction pins a cuple of nanoseconds before we step the steppers + // Set the direction pins a couple of nanoseconds before we step the steppers STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK); // Then pulse the stepping pins STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | out_bits; // Reset step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after // exactly settings.pulse_microseconds microseconds. - TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND)/8); - +// TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND)/8); + TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); // Bit shift divide by 8. + busy = true; sei(); // Re enable interrupts (normally disabled while inside an interrupt handler) // ((We re-enable interrupts in order for SIG_OVERFLOW2 to be able to be triggered @@ -172,7 +182,7 @@ SIGNAL(TIMER1_COMPA_vect) counter_z -= current_block->step_event_count; } - step_events_completed += 1; // Iterate step events + step_events_completed++; // Iterate step events // While in block steps, check for de/ac-celeration events and execute them accordingly. if (step_events_completed < current_block->step_event_count) { @@ -205,12 +215,15 @@ SIGNAL(TIMER1_COMPA_vect) } else { // Iterate cycle counter and check if speeds need to be reduced. if ( iterate_trapezoid_cycle_counter() ) { - // NOTE: We will only reduce speed if the result will be > 0. This catches small - // rounding errors that might leave steps hanging after the last trapezoid tick. - // The if statement performs a bit shift multiply by 2 to gauge when to begin - // adjusting the rate by half increments. Prevents the long slope at the end of - // deceleration issue that occurs in certain cases. - if ((trapezoid_adjusted_rate << 1) > current_block->rate_delta) { + // NOTE: We will only do a full speed reduction if the result is more than the minimum safe + // rate, initialized in trapezoid reset as 1.5 x rate_delta. Otherwise, reduce the speed by + // half increments until finished. The half increments are guaranteed not to exceed the + // CNC acceleration limits, because they will never be greater than rate_delta. This catches + // small errors that might leave steps hanging after the last trapezoid tick or a very slow + // step rate at the end of a full stop deceleration in certain situations. The half rate + // reductions should only be called once or twice per block and create a nice smooth + // end deceleration. + if (trapezoid_adjusted_rate > min_safe_rate) { trapezoid_adjusted_rate -= current_block->rate_delta; } else { trapezoid_adjusted_rate >>= 1; // Bit shift divide by 2 @@ -235,11 +248,8 @@ SIGNAL(TIMER1_COMPA_vect) current_block = NULL; plan_discard_current_block(); } - - } else { - // Still no block? Set the stepper pins to low before sleeping. - out_bits = 0; - } + + } out_bits ^= settings.invert_mask; // Apply stepper invert mask busy=false; @@ -339,3 +349,11 @@ void st_go_home() limits_go_home(); plan_set_current_position(0,0,0); } + +// Planner external interface to start stepper interrupt and execute the blocks in queue. +void st_cycle_start() { + if (!cycle_start) { + cycle_start = true; + st_wake_up(); + } +} diff --git a/stepper.h b/stepper.h index 5d5758c..73d3e7c 100644 --- a/stepper.h +++ b/stepper.h @@ -38,8 +38,7 @@ void st_synchronize(); // Execute the homing cycle void st_go_home(); -// The stepper subsystem goes to sleep when it runs out of things to execute. Call this -// to notify the subsystem that it is time to go to work. -void st_wake_up(); +// Notify the stepper subsystem to start executing the g-code program in buffer. +void st_cycle_start(); #endif