motion_control.c

Go to the documentation of this file.

/*
  motion_control.c - high level interface for issuing motion commands
  Part of Grbl

  Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
  Copyright (c) 2009-2011 Simen Svale Skogsrud

  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 <http://www.gnu.org/licenses/>.
*/

#include "grbl.h"

// unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
// (1 minute)/feed_rate time.
// 
// segments, must pass through this routine before being passed to the planner. The seperation of
// mc_line and plan_buffer_line is done primarily to place non-planner-type functions from being
// in the planner and to let backlash compensation or canned cycle integration simple and direct.
void mc_line(float *target, plan_line_data_t *pl_data)
{
// If enabled, check for soft limit violations. Placed here all line motions are picked up
// from everywhere in Grbl.
  if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) {
// 
    if (sys.state != STATE_JOG) { limits_soft_check(target); }
  }

// If in check gcode mode, prevent motion by blocking planner. Soft limits still work.
  if (sys.state == STATE_CHECK_MODE) { return; }

// 
// to insert a backlash line motion(s) before the intended line motion and will require its own
// plan_check_full_buffer() and check for system abort loop. Also for position reporting
// backlash steps will need to be also tracked, which will need to be kept at a system level.
// There are likely some other things that will need to be tracked as well. However, we feel
// that backlash compensation should NOT be handled by Grbl itself, because there are a myriad
// of ways to implement it and can be effective or ineffective for different CNC machines. This
// would be better handled by the interface as a post-processor task, where the original g-code
// is translated and inserts backlash motions that best suits the machine.
// 
// indicates to Grbl what is a backlash compensation motion, so that Grbl executes the move but
// doesn't update the machine position values. Since the position values used by the g-code
// parser and planner are separate from the system machine positions, this is doable.

// If the buffer is full: good! That means we are well ahead of the robot.
// Remain in this loop until there is room in the buffer.
  do {
    protocol_execute_realtime(); 
    if (sys.abort) { return; } 
    if ( plan_check_full_buffer() ) { protocol_auto_cycle_start(); } 
    else { break; }
  } while (1);

// Plan and queue motion into planner buffer
// uint8_t plan_status; //!< Not used in normal operation.
  plan_buffer_line(target, pl_data);
}

// offset == offset from current xyz, axis_X defines circle plane in tool space, axis_linear is
// the direction of helical travel, radius == circle radius, isclockwise boolean. Used
// for vector transformation direction.
// The arc is approximated by generating a huge number of tiny, linear segments. The chordal tolerance
// of each segment is configured in settings.arc_tolerance, which is defined to be the maximum normal
// distance from segment to the circle when the end points both lie on the circle.
void mc_arc(float *target, plan_line_data_t *pl_data, float *position, float *offset, float radius,
  uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc)
{
  float center_axis0 = position[axis_0] + offset[axis_0];
  float center_axis1 = position[axis_1] + offset[axis_1];
  float r_axis0 = -offset[axis_0];  
  float r_axis1 = -offset[axis_1];
  float rt_axis0 = target[axis_0] - center_axis0;
  float rt_axis1 = target[axis_1] - center_axis1;

// CCW angle between position and target from circle center. Only one atan2() trig computation required.
  float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
  if (is_clockwise_arc) { 
    if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= 2*M_PI; }
  } else {
    if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += 2*M_PI; }
  }

// 
// (2x) settings.arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit
// is desired, i.e. least-squares, midpoint on arc, just change the mm_per_arc_segment calculation.
// For the intended uses of Grbl, this value shouldn't exceed 2000 for the strictest of cases.
  uint16_t segments = floor(fabs(0.5*angular_travel*radius)/
                          sqrt(settings.arc_tolerance*(2*radius - settings.arc_tolerance)) );

  if (segments) {
// 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 (pl_data->condition & PL_COND_FLAG_INVERSE_TIME) { 
      pl_data->feed_rate *= segments; 
      bit_false(pl_data->condition,PL_COND_FLAG_INVERSE_TIME); 
    }
    
    float theta_per_segment = angular_travel/segments;
    float linear_per_segment = (target[axis_linear] - position[axis_linear])/segments;

// Computes: cos_T = 1 - theta_per_segment^2/2, sin_T = theta_per_segment - theta_per_segment^3/6) in ~52usec
    float cos_T = 2.0 - theta_per_segment*theta_per_segment;
    float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0);
    cos_T *= 0.5;

    float sin_Ti;
    float cos_Ti;
    float r_axisi;
    uint16_t i;
    uint8_t count = 0;

    for (i = 1; i<segments; i++) { 

      if (count < N_ARC_CORRECTION) {
// Apply vector rotation matrix. ~40 usec
        r_axisi = r_axis0*sin_T + r_axis1*cos_T;
        r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
        r_axis1 = r_axisi;
        count++;
      } else {
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. ~375 usec
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
        cos_Ti = cos(i*theta_per_segment);
        sin_Ti = sin(i*theta_per_segment);
        r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
        r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
        count = 0;
      }

// Update arc_target location
      position[axis_0] = center_axis0 + r_axis0;
      position[axis_1] = center_axis1 + r_axis1;
      position[axis_linear] += linear_per_segment;

      mc_line(position, pl_data);

// Bail mid-circle on system abort. Runtime command check already performed by mc_line.
      if (sys.abort) { return; }
    }
  }
// Ensure last segment arrives at target location.
  mc_line(target, pl_data);
}

void mc_dwell(float seconds)
{
  if (sys.state == STATE_CHECK_MODE) { return; }
  protocol_buffer_synchronize();
  delay_sec(seconds, DELAY_MODE_DWELL);
}

// 
// executing the homing cycle. This prevents incorrect buffered plans after homing.
void mc_homing_cycle(uint8_t cycle_mask)
{
// Check and abort homing cycle, if hard limits are already enabled. Helps prevent problems
// with machines with limits wired on both ends of travel to one limit pin.
  #ifdef LIMITS_TWO_SWITCHES_ON_AXES
    if (limits_get_state()) {
      mc_reset(); 
      system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT);
      return;
    }
  #endif

  limits_disable(); 

// -------------------------------------------------------------------------------------
// Perform homing routine. 
  
  #ifdef HOMING_SINGLE_AXIS_COMMANDS
    if (cycle_mask) { limits_go_home(cycle_mask); } 
    else
  #endif
  {
// Search to engage all axes limit switches at faster homing seek rate.
    limits_go_home(HOMING_CYCLE_0);  
    #ifdef HOMING_CYCLE_1
      limits_go_home(HOMING_CYCLE_1);  
    #endif
    #ifdef HOMING_CYCLE_2
      limits_go_home(HOMING_CYCLE_2);  
    #endif
  }

  protocol_execute_realtime(); 
  if (sys.abort) { return; } 

// Homing cycle complete! Setup system for normal operation.
// -------------------------------------------------------------------------------------

// Sync gcode parser and planner positions to homed position.
  gc_sync_position();
  plan_sync_position();

// If hard limits feature enabled, re-enable hard limits pin change register after homing cycle.
  limits_init();
}

// 
uint8_t mc_probe_cycle(float *target, plan_line_data_t *pl_data, uint8_t parser_flags)
{
  if (sys.state == STATE_CHECK_MODE) { return(GC_PROBE_CHECK_MODE); }

// Finish all queued commands and empty planner buffer before starting probe cycle.
  protocol_buffer_synchronize();
  if (sys.abort) { return(GC_PROBE_ABORT); } 

// Initialize probing control variables
  uint8_t is_probe_away = bit_istrue(parser_flags,GC_PARSER_PROBE_IS_AWAY);
  uint8_t is_no_error = bit_istrue(parser_flags,GC_PARSER_PROBE_IS_NO_ERROR);
  sys.probe_succeeded = false; 
  probe_configure_invert_mask(is_probe_away);

// After syncing, check if probe is already triggered. If so, halt and issue alarm.
// 
  if ( probe_get_state() ) { 
    system_set_exec_alarm(EXEC_ALARM_PROBE_FAIL_INITIAL);
    protocol_execute_realtime();
    probe_configure_invert_mask(false); 
    return(GC_PROBE_FAIL_INIT); 
  }

// Setup and queue probing motion. Auto cycle-start should not start the cycle.
  mc_line(target, pl_data);

// Activate the probing state monitor in the stepper module.
  sys_probe_state = PROBE_ACTIVE;

// Perform probing cycle. Wait here until probe is triggered or motion completes.
  system_set_exec_state_flag(EXEC_CYCLE_START);
  do {
    protocol_execute_realtime();
    if (sys.abort) { return(GC_PROBE_ABORT); } 
  } while (sys.state != STATE_IDLE);

// Probing cycle complete!

// Set state variables and error out, if the probe failed and cycle with error is enabled.
  if (sys_probe_state == PROBE_ACTIVE) {
    if (is_no_error) { memcpy(sys_probe_position, sys_position, sizeof(sys_position)); }
    else { system_set_exec_alarm(EXEC_ALARM_PROBE_FAIL_CONTACT); }
  } else {
    sys.probe_succeeded = true; 
  }
  sys_probe_state = PROBE_OFF; 
  probe_configure_invert_mask(false); 
  protocol_execute_realtime();   

// Reset the stepper and planner buffers to remove the remainder of the probe motion.
  st_reset(); 
  plan_reset(); 
  plan_sync_position(); 

  #ifdef MESSAGE_PROBE_COORDINATES
// All done! Output the probe position as message.
    report_probe_parameters();
  #endif

  if (sys.probe_succeeded) { return(GC_PROBE_FOUND); } 
  else { return(GC_PROBE_FAIL_END); } 
}

// 
void mc_parking_motion(float *parking_target, plan_line_data_t *pl_data)
{
  if (sys.abort) { return; } 

  uint8_t plan_status = plan_buffer_line(parking_target, pl_data);

  if (plan_status) {
        bit_true(sys.step_control, STEP_CONTROL_EXECUTE_SYS_MOTION);
        bit_false(sys.step_control, STEP_CONTROL_END_MOTION); 
    st_parking_setup_buffer(); 
        st_prep_buffer();
        st_wake_up();
        do {
            protocol_exec_rt_system();
            if (sys.abort) { return; }
        } while (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION);
        st_parking_restore_buffer(); 
    } else {
    bit_false(sys.step_control, STEP_CONTROL_EXECUTE_SYS_MOTION);
        protocol_exec_rt_system();
  }

}

// active processes in the system. This also checks if a system reset is issued while Grbl
// is in a motion state. If so, kills the steppers and sets the system alarm to flag position
// lost, since there was an abrupt uncontrolled deceleration. Called at an interrupt level by
// realtime abort command and hard limits. So, keep to a minimum.
void mc_reset()
{
// Only this function can set the system reset. Helps prevent multiple kill calls.
  if (bit_isfalse(sys_rt_exec_state, EXEC_RESET)) {
    system_set_exec_state_flag(EXEC_RESET);

// Kill spindle and coolant.
    spindle_stop();
    coolant_stop();

// Kill steppers only if in any motion state, i.e. cycle, actively holding, or homing.
// 
// the steppers enabled by avoiding the go_idle call altogether, unless the motion state is
// violated, by which, all bets are off.
    if ((sys.state & (STATE_CYCLE | STATE_HOMING | STATE_JOG)) ||
            (sys.step_control & (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION))) {
      if (sys.state == STATE_HOMING) { 
        if (!sys_rt_exec_alarm) {system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_RESET); }
      } else { system_set_exec_alarm(EXEC_ALARM_ABORT_CYCLE); }
      st_go_idle(); 
    }
  }
}