运动控制 motion_control.c - beginner-lei/GRBL GitHub Wiki

motion_control.c

include "grbl.h"

//以绝对毫米坐标执行直线运动。给进给速率以毫米/秒。除非invert_feed_rate为真。然后feed_rate表示运动应该在(1分钟)/feed_rate时间内完成。

//注意:这是grbl规划器的主网关。所有的直线运动，包括弧线段，在传递给规划器之前必须通过这个程序。

将 mc_line和plan_buffer_line分离主要是为了将非规划器类型的函数从规划器中分离出来，并让反弹补偿或固定循环集成简单而直接。

ifdef USE_LINE_NUMBERS

  void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number)

else

  void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate)

endif

{

  //如果启用，检查软限制违规。放置在这里，所有的直线运动都被从Grbl的任何地方收集起来

//如果在检查gcode模式，阻止运动通过阻塞规划器。软限制仍然有效。

//注意:侧隙补偿可以安装在这里。它将需要方向信息来跟踪在预期的线运动之前插入一个反弹线运动(s)，并需要它自己的plan_check_full_buffer()和检查系统中止循环。同样对于位置报告反弹步骤也需要被跟踪，这将需要保持在一个系统级别。可能还有其他一些事情需要跟踪。然而，我们认为，反弹补偿不应该由Grbl本身处理，因为有无数的方法来实现它，可以有效或无效的不同的数控机床。这个将更好地由界面作为后处理任务处理，在那里原始g-code 被翻译和插入最适合机器的反弹运动。

//注意:可能作为一个中间立场，所有需要发送的是一个标志或特殊命令，指示Grbl什么是反弹补偿运动，这样Grbl执行移动，但不更新机器位置值。因为g-code解析器和规划器使用的位置值与系统机器位置是分开的，所以这是可行的。如果缓冲区已满:很好!这意味着我们远远领先于机器人。保持在这个循环中，直到缓冲区中有空间。

  do {
    protocol_execute_realtime(); // 检查任何运行时命令
    if (sys.abort) { return; } // 如果系统中止，保释。
    if ( plan_check_full_buffer() ) { protocol_auto_cycle_start(); } //当缓冲区满时自动循环开始
    else { break; }
  } while (1);

  // 计划和队列运动到计划器缓冲区

  #ifdef USE_LINE_NUMBERS
    plan_buffer_line(target, feed_rate, invert_feed_rate, line_number);
  #else
    plan_buffer_line(target, feed_rate, invert_feed_rate);
  #endif

}

//以偏移模式格式执行圆弧。position ==当前xyz, target ==目标xyz，offset ==当前xyz的偏移量，axis_X定义工具空间中的圆平面， axis_linear是螺旋运动方向，半径==圆半径，顺时针布尔值。用于向量变换方向。通过生成大量微小的线段来近似弧形。每个段的和弦公差在设置中配置。Arc_tolerance，定义为结束点都在圆上时，线段到圆的最大法线距离。

ifdef USE_LINE_NUMBERS

  void mc_arc(float *position, float *target, float *offset, float radius, float feed_rate, 
    uint8_t invert_feed_rate, uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc, int32_t line_number)

else

  void mc_arc(float *position, float *target, float *offset, float radius, float feed_rate,
    uint8_t invert_feed_rate, uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc)

endif

{

  //从圆心开始的位置和目标之间的CCW角。只需要一个atan2()三角运算。

  float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
  if (is_clockwise_arc) { // Correct atan2 output per direction
    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; }
  }

  if (segments) {

    //乘以逆feed_rate来补偿这个运动是由一些离散的片段来近似的。逆feed_rate对于所有分段的和应该是正确的。

    theta_per_segment = angular_travel/segments;
    linear_per_segment = (target[axis_linear] - position[axis_linear])/segments;

/*通过变换矩阵进行向量旋转:r为原向量，r_T为旋转向量，为旋转角度。Jens Geisler的解决方法。 r_T = cos() -sin()Sin cos r;圆弧生成以圆心为旋转轴，定义从圆心到初始位置的半径矢量。每条线段都是由连续的矢量旋转构成的。在极少数情况下，单个精度值积累的误差可能大于工具精度。实现了精确的圆弧路径修正。这种方法避免了太多非常昂贵的三角运算[sin()，cos()，tan()]的问题，每个三角运算可能需要100-200次。小角度近似可以进一步减少计算开销。三阶近似(二阶sin()有太多的误差)适用于大多数，如果不是，所有CNC应用程序。请注意，当ta_per_segment大于~0.25 rad(14 deg)时，这种近似将开始积累一个数值漂移误差，并且这种近似被连续使用几十次而没有校正。这种情况是极不可能的，因为段长度和ta_per_segment是自动生成的，并通过弧容差设置缩放。只有非常大的弧公差设置，不现实的数控应用，将导致这种数字漂移误差。但是，最好将N_ARC_CORRECTION从~4的低值设置为~20左右，以避免三角运算，同时保持电弧生成的准确性。这种近似方法还允许mc_arc立即将线段插入到规划器中，而无需计算cos()或sin()的初始开销。当需要对弧线进行修正时，规划人员应该已经赶上了由初始mc_arc开销引起的延迟。当有连续的弧运动时，这是很重要的。*/ //计算:cos_T = 1 - theta_per_segment^2/2, sin_T = theta_per_segment - theta_per_segment^3/6)在~52usec

    for (i = 1; i<segments; i++) { // Increment (segments-1).增量(segments-1)。
      
      if (count < N_ARC_CORRECTION) {
        //应用矢量旋转矩阵。~ 40微秒
        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 {

//半径矢量的圆弧校正。只计算每次N_ARC_CORRECTION增量。~375 usec //利用初始半径矢量(=-offset)的变换矩阵计算精确位置。

      // 更新arc_target位置
      position[axis_0] = center_axis0 + r_axis0;
      position[axis_1] = center_axis1 + r_axis1;
      position[axis_linear] += linear_per_segment;
      
      #ifdef USE_LINE_NUMBERS
        mc_line(position, feed_rate, invert_feed_rate, line_number);
      #else
        mc_line(position, feed_rate, invert_feed_rate);
      #endif
      
   
      //系统中止中圈保释。mc_line已经执行运行时命令检查。
      if (sys.abort) { return; }
    }
  }
  // 确保最后一段到达目标位置。
  #ifdef USE_LINE_NUMBERS
    mc_line(target, feed_rate, invert_feed_rate, line_number);
  #else
    mc_line(target, feed_rate, invert_feed_rate);
  #endif

}

//以秒为单位执行驻留。 void mc_dwell(float seconds) {

   i = floor(1000/DWELL_TIME_STEP*seconds);
   protocol_buffer_synchronize();
   delay_ms(floor(1000*seconds-i*DWELL_TIME_STEP)); //延迟毫秒余数
   while (i-- > 0) {

     //注意:在dwell期间每毫秒<= dwell _time_step检查并执行实时命令。
     protocol_execute_realtime();
     if (sys.abort) { return; }
     delay_ms(DWELL_TIME_STEP); // 延迟DWELL_TIME_STEP增量
   }

}

//执行归一循环定位和设置机器零位。只有'$H'执行此命令。 //注意:在执行归位周期之前，缓冲区应该没有运动，Grbl必须处于空闲状态。这可以防止归航后不正确的缓冲计划。

void mc_homing_cycle(void) {

  //检查并中止返航周期，如果硬限制已经启用，则检查并中止归航周期。帮助防止问题的机器与限制有线两端旅行到一个限制针。
  // TODO:将pin特定的LIMIT_PIN调用作为一个函数移动到limits.c。

  limits_disable(); //禁用周期持续时间的硬限制引脚更改寄存器
    
  // -------------------------------------------------------------------------------------
  //执行归位程序。注意:特殊运动情况。只有系统复位生效。
  //搜索以更快的寻迹速度连接所有轴限位开关。

  protocol_execute_realtime(); //检查复位和设置系统中止。
  if (sys.abort) { return; } //没有完成。mc_alarm设置的告警状态。

  //归航周期完成!设置系统正常运行。
  // -------------------------------------------------------------------------------------

  // Gcode解析器位置被limits_go_home()例程所回避，所以现在是同步位置。

  gc_sync_position();

  //如果硬限制功能已启用，请在返回周期后重新启用硬限制引脚更改寄存器。

  limits_init();

}

//执行工具长度探测周期。需要探测开关。 //注意:探测失败时，程序将停止并进入ALARM状态。

ifdef USE_LINE_NUMBERS

  void mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate, uint8_t is_probe_away, 
    uint8_t is_no_error, int32_t line_number)

else

  void mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate, uint8_t is_probe_away,
    uint8_t is_no_error)

endif

{

  // TODO:需要更新这个循环，所以它遵循一个非自动循环启动。
  if (sys.state == STATE_CHECK_MODE) { return; }

  //在开始探测周期之前，完成所有排队命令和清空规划器缓冲区。
  protocol_buffer_synchronize();

  //同步后，检查探针是否已经触发。如果是，停止并发出警报。
  //注意:这个探测初始化错误适用于所有探测周期。
  if ( probe_get_state() ) { // Check probe pin state.检查探针引脚状态
    bit_true_atomic(sys_rt_exec_alarm, EXEC_ALARM_PROBE_FAIL);
    protocol_execute_realtime();
  }
  if (sys.abort) { return; } // Return if system reset has been issued.如果已发出系统重置，则返回。

  //激活步进模块中的探测状态监视器。

  sys_probe_state = PROBE_ACTIVE;

  //执行探测周期。在这里等待，直到探针被触发或动作完成。
  bit_true_atomic(sys_rt_exec_state, EXEC_CYCLE_START);
  do {
    protocol_execute_realtime(); 
    if (sys.abort) { return; } //检查系统中止
  } while (sys.state != STATE_IDLE);
  
  // 调查周期完成!
  //设置状态变量和error out，如果探测失败，并启用了cycle with error。

  if (sys_probe_state == PROBE_ACTIVE) {
    if (is_no_error) { memcpy(sys.probe_position, sys.position, sizeof(float)*N_AXIS); }
    else { bit_true_atomic(sys_rt_exec_alarm, EXEC_ALARM_PROBE_FAIL); }
  } else { 
    sys.probe_succeeded = true; //向系统表示探测周期已成功完成
  }
  sys_probe_state = PROBE_OFF; //确保探针状态监视器被禁用
  protocol_execute_realtime();   //检查并执行运行时命令
  if (sys.abort) { return; } // 检查系统中止

  // 重置步进器和计划器缓冲区以移除探头运动的剩余部分。
  st_reset(); // 休息步骤段缓冲区
  plan_reset(); //重置规划师缓冲区。零计划的位置。确保探测运动被清除
  plan_sync_position(); // 同步计划位置到当前机器位置

  // TODO:更新g-code解析器代码，不需要这个目标计算，而是使用gc_sync_position()调用。
  //注意:这里更新的target[]变量将被返回并与g-code解析器同步。

  system_convert_array_steps_to_mpos(target, sys.position);

  #ifdef MESSAGE_PROBE_COORDINATES

//全部完成!将探针位置输出为消息。

    report_probe_parameters();
  #endif

}

//通过设置realtime reset命令并杀死系统中的任何活动进程来准备系统重置的方法。这也检查当Grbl 处于运动状态时是否发出系统重置。如果是这样，关闭步进器，并将系统报警设置为标志位置丢失，因为有一个突然的不受控制的减速。在中断级别由realtime中止命令和硬限制调用。所以，保持最低限度。

void mc_reset(void) {

    //压下主轴和冷却剂。

    spindle_stop();
    coolant_stop();

    //只有在任何运动状态下，即循环，主动保持，或归位时才终止步进者。

    //注意:如果步进器通过步骤空闲延迟设置保持启用，这也通过避免go_idle调用保持步进器启用，除非运动状态违反，除非违反了运动状态，否则所有的赌注都失败了

    if ((sys.state & (STATE_CYCLE | STATE_HOMING)) || (sys.suspend == SUSPEND_ENABLE_HOLD)) {
      if (sys.state == STATE_HOMING) { bit_true_atomic(sys_rt_exec_alarm, EXEC_ALARM_HOMING_FAIL); }
      else { bit_true_atomic(sys_rt_exec_alarm, EXEC_ALARM_ABORT_CYCLE); }
      st_go_idle(); // Force kill steppers. Position has likely been lost./强制杀步进器。可能已经失去了位置
    }
  }

}

运动控制 motion_control.c - beginner-lei/GRBL GitHub Wiki

⚠️ **GitHub.com Fallback** ⚠️

⚠️ GitHub.com Fallback ⚠️