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