diff --git a/motion_control_new.c b/motion_control_new.c deleted file mode 100644 index 6ca0c9f..0000000 --- a/motion_control_new.c +++ /dev/null @@ -1,207 +0,0 @@ -/* - 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; i