motion_control.c

00001 /*
00002   motion_control.c - high level interface for issuing motion commands
00003   Part of Grbl
00004 
00005   Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
00006   Copyright (c) 2009-2011 Simen Svale Skogsrud
00007 
00008   Grbl is free software: you can redistribute it and/or modify
00009   it under the terms of the GNU General Public License as published by
00010   the Free Software Foundation, either version 3 of the License, or
00011   (at your option) any later version.
00012 
00013   Grbl is distributed in the hope that it will be useful,
00014   but WITHOUT ANY WARRANTY; without even the implied warranty of
00015   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016   GNU General Public License for more details.
00017 
00018   You should have received a copy of the GNU General Public License
00019   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
00020 */
00021 
00022 #include "grbl.h"
00023 
00024 
00025 // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
00026 // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
00027 // (1 minute)/feed_rate time.
00028 // NOTE: This is the primary gateway to the grbl planner. All line motions, including arc line
00029 // segments, must pass through this routine before being passed to the planner. The seperation of
00030 // mc_line and plan_buffer_line is done primarily to place non-planner-type functions from being
00031 // in the planner and to let backlash compensation or canned cycle integration simple and direct.
00032 void mc_line(float *target, plan_line_data_t *pl_data)
00033 {
00034   // If enabled, check for soft limit violations. Placed here all line motions are picked up
00035   // from everywhere in Grbl.
00036   if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) {
00037     // NOTE: Block jog state. Jogging is a special case and soft limits are handled independently.
00038     if (sys.state != STATE_JOG) { limits_soft_check(target); }
00039   }
00040 
00041   // If in check gcode mode, prevent motion by blocking planner. Soft limits still work.
00042   if (sys.state == STATE_CHECK_MODE) { return; }
00043 
00044   // NOTE: Backlash compensation may be installed here. It will need direction info to track when
00045   // to insert a backlash line motion(s) before the intended line motion and will require its own
00046   // plan_check_full_buffer() and check for system abort loop. Also for position reporting
00047   // backlash steps will need to be also tracked, which will need to be kept at a system level.
00048   // There are likely some other things that will need to be tracked as well. However, we feel
00049   // that backlash compensation should NOT be handled by Grbl itself, because there are a myriad
00050   // of ways to implement it and can be effective or ineffective for different CNC machines. This
00051   // would be better handled by the interface as a post-processor task, where the original g-code
00052   // is translated and inserts backlash motions that best suits the machine.
00053   // NOTE: Perhaps as a middle-ground, all that needs to be sent is a flag or special command that
00054   // indicates to Grbl what is a backlash compensation motion, so that Grbl executes the move but
00055   // doesn't update the machine position values. Since the position values used by the g-code
00056   // parser and planner are separate from the system machine positions, this is doable.
00057 
00058   // If the buffer is full: good! That means we are well ahead of the robot.
00059   // Remain in this loop until there is room in the buffer.
00060   do {
00061     protocol_execute_realtime(); // Check for any run-time commands
00062     if (sys.abort) { return; } // Bail, if system abort.
00063     if ( plan_check_full_buffer() ) { protocol_auto_cycle_start(); } // Auto-cycle start when buffer is full.
00064     else { break; }
00065   } while (1);
00066 
00067   // Plan and queue motion into planner buffer
00068     if (plan_buffer_line(target, pl_data) == PLAN_EMPTY_BLOCK) {
00069         if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) {
00070             // Correctly set spindle state, if there is a coincident position passed. Forces a buffer
00071             // sync while in M3 laser mode only.
00072             if (pl_data->condition & PL_COND_FLAG_SPINDLE_CW) {
00073                 spindle_sync(PL_COND_FLAG_SPINDLE_CW, pl_data->spindle_speed);
00074             }
00075         }
00076     }
00077 }
00078 
00079 
00080 // Execute an arc in offset mode format. position == current xyz, target == target xyz,
00081 // offset == offset from current xyz, axis_X defines circle plane in tool space, axis_linear is
00082 // the direction of helical travel, radius == circle radius, isclockwise boolean. Used
00083 // for vector transformation direction.
00084 // The arc is approximated by generating a huge number of tiny, linear segments. The chordal tolerance
00085 // of each segment is configured in settings.arc_tolerance, which is defined to be the maximum normal
00086 // distance from segment to the circle when the end points both lie on the circle.
00087 void mc_arc(float *target, plan_line_data_t *pl_data, float *position, float *offset, float radius,
00088   uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc)
00089 {
00090   float center_axis0 = position[axis_0] + offset[axis_0];
00091   float center_axis1 = position[axis_1] + offset[axis_1];
00092   float r_axis0 = -offset[axis_0];  // Radius vector from center to current location
00093   float r_axis1 = -offset[axis_1];
00094   float rt_axis0 = target[axis_0] - center_axis0;
00095   float rt_axis1 = target[axis_1] - center_axis1;
00096 
00097   // CCW angle between position and target from circle center. Only one atan2() trig computation required.
00098   float angular_travel = atan2f(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
00099   if (is_clockwise_arc) { // Correct atan2 output per direction
00100     if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= 2*M_PI; }
00101   } else {
00102     if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += 2*M_PI; }
00103   }
00104 
00105   // NOTE: Segment end points are on the arc, which can lead to the arc diameter being smaller by up to
00106   // (2x) settings.arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit
00107   // is desired, i.e. least-squares, midpoint on arc, just change the mm_per_arc_segment calculation.
00108   // For the intended uses of Grbl, this value shouldn't exceed 2000 for the strictest of cases.
00109   uint16_t segments = (uint16_t)floorf(fabsf(0.5f*angular_travel*radius) /
00110                           sqrtf(settings.arc_tolerance*(2*radius - settings.arc_tolerance)) );
00111 
00112   if (segments) {
00113     // Multiply inverse feed_rate to compensate for the fact that this movement is approximated
00114     // by a number of discrete segments. The inverse feed_rate should be correct for the sum of
00115     // all segments.
00116     if (pl_data->condition & PL_COND_FLAG_INVERSE_TIME) { 
00117       pl_data->feed_rate *= segments; 
00118       bit_false(pl_data->condition,PL_COND_FLAG_INVERSE_TIME); // Force as feed absolute mode over arc segments.
00119     }
00120     
00121     float theta_per_segment = angular_travel/segments;
00122     float linear_per_segment = (target[axis_linear] - position[axis_linear])/segments;
00123 
00124     /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
00125        and phi is the angle of rotation. Solution approach by Jens Geisler.
00126            r_T = [cos(phi) -sin(phi);
00127                   sin(phi)  cos(phi] * r ;
00128 
00129        For arc generation, the center of the circle is the axis of rotation and the radius vector is
00130        defined from the circle center to the initial position. Each line segment is formed by successive
00131        vector rotations. Single precision values can accumulate error greater than tool precision in rare
00132        cases. So, exact arc path correction is implemented. This approach avoids the problem of too many very
00133        expensive trig operations [sin(),cos(),tan()] which can take 100-200 usec each to compute.
00134 
00135        Small angle approximation may be used to reduce computation overhead further. A third-order approximation
00136        (second order sin() has too much error) holds for most, if not, all CNC applications. Note that this
00137        approximation will begin to accumulate a numerical drift error when theta_per_segment is greater than
00138        ~0.25 rad(14 deg) AND the approximation is successively used without correction several dozen times. This
00139        scenario is extremely unlikely, since segment lengths and theta_per_segment are automatically generated
00140        and scaled by the arc tolerance setting. Only a very large arc tolerance setting, unrealistic for CNC
00141        applications, would cause this numerical drift error. However, it is best to set N_ARC_CORRECTION from a
00142        low of ~4 to a high of ~20 or so to avoid trig operations while keeping arc generation accurate.
00143 
00144        This approximation also allows mc_arc to immediately insert a line segment into the planner
00145        without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
00146        a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
00147        This is important when there are successive arc motions.
00148     */
00149     // Computes: cos_T = 1 - theta_per_segment^2/2, sin_T = theta_per_segment - theta_per_segment^3/6) in ~52usec
00150     float cos_T = 2.0f - theta_per_segment*theta_per_segment;
00151     float sin_T = theta_per_segment*0.16666667f*(cos_T + 4.0f);
00152     cos_T *= 0.5;
00153 
00154     float sin_Ti;
00155     float cos_Ti;
00156     float r_axisi;
00157     uint16_t i;
00158     uint8_t count = 0;
00159 
00160     for (i = 1; i<segments; i++) { // Increment (segments-1).
00161 
00162       if (count < N_ARC_CORRECTION) {
00163         // Apply vector rotation matrix. ~40 usec
00164         r_axisi = r_axis0*sin_T + r_axis1*cos_T;
00165         r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
00166         r_axis1 = r_axisi;
00167         count++;
00168       } else {
00169         // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. ~375 usec
00170         // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
00171         cos_Ti = cosf(i*theta_per_segment);
00172         sin_Ti = sinf(i*theta_per_segment);
00173         r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
00174         r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
00175         count = 0;
00176       }
00177 
00178       // Update arc_target location
00179       position[axis_0] = center_axis0 + r_axis0;
00180       position[axis_1] = center_axis1 + r_axis1;
00181       position[axis_linear] += linear_per_segment;
00182 
00183       mc_line(position, pl_data);
00184 
00185       // Bail mid-circle on system abort. Runtime command check already performed by mc_line.
00186       if (sys.abort) { return; }
00187     }
00188   }
00189   // Ensure last segment arrives at target location.
00190   mc_line(target, pl_data);
00191 }
00192 
00193 
00194 // Execute dwell in seconds.
00195 void mc_dwell(float seconds)
00196 {
00197   if (sys.state == STATE_CHECK_MODE) { return; }
00198   protocol_buffer_synchronize();
00199   delay_sec(seconds, DELAY_MODE_DWELL);
00200 }
00201 
00202 
00203 // Perform homing cycle to locate and set machine zero. Only '$H' executes this command.
00204 // NOTE: There should be no motions in the buffer and Grbl must be in an idle state before
00205 // executing the homing cycle. This prevents incorrect buffered plans after homing.
00206 void mc_homing_cycle(uint8_t cycle_mask)
00207 {
00208   // Check and abort homing cycle, if hard limits are already enabled. Helps prevent problems
00209   // with machines with limits wired on both ends of travel to one limit pin.
00210   // TODO: Move the pin-specific LIMIT_PIN call to limits.c as a function.
00211   #ifdef LIMITS_TWO_SWITCHES_ON_AXES
00212     if (limits_get_state()) {
00213       mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown.
00214       system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT);
00215       return;
00216     }
00217   #endif
00218 
00219   limits_disable(); // Disable hard limits pin change register for cycle duration
00220 
00221   // -------------------------------------------------------------------------------------
00222   // Perform homing routine. NOTE: Special motion case. Only system reset works.
00223   
00224   #ifdef HOMING_SINGLE_AXIS_COMMANDS
00225     if (cycle_mask) { limits_go_home(cycle_mask); } // Perform homing cycle based on mask.
00226     else
00227   #endif
00228   {
00229     // Search to engage all axes limit switches at faster homing seek rate.
00230     limits_go_home(HOMING_CYCLE_0);  // Homing cycle 0
00231     #ifdef HOMING_CYCLE_1
00232       limits_go_home(HOMING_CYCLE_1);  // Homing cycle 1
00233     #endif
00234     #ifdef HOMING_CYCLE_2
00235       limits_go_home(HOMING_CYCLE_2);  // Homing cycle 2
00236     #endif
00237   }
00238 
00239   protocol_execute_realtime(); // Check for reset and set system abort.
00240   if (sys.abort) { return; } // Did not complete. Alarm state set by mc_alarm.
00241 
00242   // Homing cycle complete! Setup system for normal operation.
00243   // -------------------------------------------------------------------------------------
00244 
00245   // Sync gcode parser and planner positions to homed position.
00246   gc_sync_position();
00247   plan_sync_position();
00248 
00249   // If hard limits feature enabled, re-enable hard limits pin change register after homing cycle.
00250 #ifdef STM32F103C8
00251     EXTI_ClearITPendingBit((1 << X_LIMIT_BIT) | (1 << Y_LIMIT_BIT) | (1 << Z_LIMIT_BIT));
00252     NVIC_ClearPendingIRQ(EXTI15_10_IRQn);
00253     NVIC_EnableIRQ(EXTI15_10_IRQn);
00254 #else
00255     limits_init();
00256 #endif
00257 }
00258 
00259 
00260 // Perform tool length probe cycle. Requires probe switch.
00261 // NOTE: Upon probe failure, the program will be stopped and placed into ALARM state.
00262 uint8_t mc_probe_cycle(float *target, plan_line_data_t *pl_data, uint8_t parser_flags)
00263 {
00264   // TODO: Need to update this cycle so it obeys a non-auto cycle start.
00265   if (sys.state == STATE_CHECK_MODE) { return(GC_PROBE_CHECK_MODE); }
00266 
00267   // Finish all queued commands and empty planner buffer before starting probe cycle.
00268   protocol_buffer_synchronize();
00269   if (sys.abort) { return(GC_PROBE_ABORT); } // Return if system reset has been issued.
00270 
00271   // Initialize probing control variables
00272   uint8_t is_probe_away = bit_istrue(parser_flags, GC_PARSER_PROBE_IS_AWAY);
00273   uint8_t is_no_error = bit_istrue(parser_flags, GC_PARSER_PROBE_IS_NO_ERROR);
00274   sys.probe_succeeded = false; // Re-initialize probe history before beginning cycle.
00275   probe_configure_invert_mask(is_probe_away);
00276 
00277   // After syncing, check if probe is already triggered. If so, halt and issue alarm.
00278   // NOTE: This probe initialization error applies to all probing cycles.
00279   if ( probe_get_state() ) { // Check probe pin state.
00280     system_set_exec_alarm(EXEC_ALARM_PROBE_FAIL_INITIAL);
00281     protocol_execute_realtime();
00282     probe_configure_invert_mask(false); // Re-initialize invert mask before returning.
00283     return(GC_PROBE_FAIL_INIT); // Nothing else to do but bail.
00284   }
00285 
00286   // Setup and queue probing motion. Auto cycle-start should not start the cycle.
00287   mc_line(target, pl_data);
00288 
00289   // Activate the probing state monitor in the stepper module.
00290   sys_probe_state = PROBE_ACTIVE;
00291 
00292   // Perform probing cycle. Wait here until probe is triggered or motion completes.
00293   system_set_exec_state_flag(EXEC_CYCLE_START);
00294   do {
00295     protocol_execute_realtime();
00296     if (sys.abort) { return(GC_PROBE_ABORT); } // Check for system abort
00297   } while (sys.state != STATE_IDLE);
00298 
00299   // Probing cycle complete!
00300 
00301   // Set state variables and error out, if the probe failed and cycle with error is enabled.
00302   if (sys_probe_state == PROBE_ACTIVE) {
00303     if (is_no_error) { memcpy(sys_probe_position, sys_position, sizeof(sys_position)); }
00304     else { system_set_exec_alarm(EXEC_ALARM_PROBE_FAIL_CONTACT); }
00305   } else {
00306     sys.probe_succeeded = true; // Indicate to system the probing cycle completed successfully.
00307   }
00308   sys_probe_state = PROBE_OFF; // Ensure probe state monitor is disabled.
00309   probe_configure_invert_mask(false); // Re-initialize invert mask.
00310   protocol_execute_realtime();   // Check and execute run-time commands
00311 
00312   // Reset the stepper and planner buffers to remove the remainder of the probe motion.
00313   st_reset(); // Reset step segment buffer.
00314   plan_reset(); // Reset planner buffer. Zero planner positions. Ensure probing motion is cleared.
00315   plan_sync_position(); // Sync planner position to current machine position.
00316 
00317   #ifdef MESSAGE_PROBE_COORDINATES
00318     // All done! Output the probe position as message.
00319     report_probe_parameters();
00320   #endif
00321 
00322   if (sys.probe_succeeded) { return(GC_PROBE_FOUND); } // Successful probe cycle.
00323   else { return(GC_PROBE_FAIL_END); } // Failed to trigger probe within travel. With or without error.
00324 }
00325 
00326 #ifdef PARKING_ENABLE
00327     void mc_parking_motion(float *parking_target, plan_line_data_t *pl_data)
00328     {
00329         if (sys.abort) { return; } // Block during abort.
00330 
00331         uint8_t plan_status = plan_buffer_line(parking_target, pl_data);
00332 
00333         if (plan_status) {
00334             bit_true(sys.step_control, STEP_CONTROL_EXECUTE_SYS_MOTION);
00335             bit_false(sys.step_control, STEP_CONTROL_END_MOTION); // Allow parking motion to execute, if feed hold is active.
00336             st_parking_setup_buffer(); // Setup step segment buffer for special parking motion case
00337             st_prep_buffer();
00338             st_wake_up();
00339             do {
00340                 protocol_exec_rt_system();
00341                 if (sys.abort) { return; }
00342             } while (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION);
00343             st_parking_restore_buffer(); // Restore step segment buffer to normal run state.
00344         }
00345         else {
00346             bit_false(sys.step_control, STEP_CONTROL_EXECUTE_SYS_MOTION);
00347             protocol_exec_rt_system();
00348         }
00349 
00350     }
00351 #endif
00352 
00353 
00354 #ifdef ENABLE_PARKING_OVERRIDE_CONTROL
00355 void mc_override_ctrl_update(uint8_t override_state)
00356 {
00357     // Finish all queued commands before altering override control state
00358     protocol_buffer_synchronize();
00359     if (sys.abort) { return; }
00360     sys.override_ctrl = override_state;
00361 }
00362 #endif
00363 // Method to ready the system to reset by setting the realtime reset command and killing any
00364 // active processes in the system. This also checks if a system reset is issued while Grbl
00365 // is in a motion state. If so, kills the steppers and sets the system alarm to flag position
00366 // lost, since there was an abrupt uncontrolled deceleration. Called at an interrupt level by
00367 // realtime abort command and hard limits. So, keep to a minimum.
00368 void mc_reset()
00369 {
00370   // Only this function can set the system reset. Helps prevent multiple kill calls.
00371   if (bit_isfalse(sys_rt_exec_state, EXEC_RESET)) {
00372     system_set_exec_state_flag(EXEC_RESET);
00373 
00374     // Kill spindle and coolant.
00375     spindle_stop();
00376     coolant_stop();
00377 
00378     // Kill steppers only if in any motion state, i.e. cycle, actively holding, or homing.
00379     // NOTE: If steppers are kept enabled via the step idle delay setting, this also keeps
00380     // the steppers enabled by avoiding the go_idle call altogether, unless the motion state is
00381     // violated, by which, all bets are off.
00382     if ((sys.state & (STATE_CYCLE | STATE_HOMING | STATE_JOG)) ||
00383             (sys.step_control & (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION))) {
00384       if (sys.state == STATE_HOMING) {
00385         if (!sys_rt_exec_alarm) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_RESET); }
00386       }
00387       else { system_set_exec_alarm(EXEC_ALARM_ABORT_CYCLE); }
00388       st_go_idle(); // Force kill steppers. Position has likely been lost.
00389     }
00390   }
00391 }