Marlin 2.1.2 运动控制部分内容分析

1、Gcode获取、分析

1.1、主循环loop()流程

1.2、流程分析

1.2.1、Gcode获取

​ 通过idle()循环，获取一串Gcode命令，然后调用queue.cpp中的（get_serial_command()&&get_sdcard_commmand())，逐个将其存入line_buffer,读取完成后再将整条命令存入环形队列ring_buffer。

1.2.2、Gcode分析和执行

​ 通过queue.advance()从ring_buffer中取出队尾Gcode指令，经过parser(解析器)解析后送到process_parsed_command()执行。

2、速度预处理流程

2.1、流程概述

​ 通过loop()循环进行Gcode命令的执行，G0、G1为直线插补命令，G2、G3圆弧插补命令，将圆弧或者直线分成N个小线段，然后对每个线段进行直线规划，计算移动过程中速度和加速度的值，同时确定两线段之间的最大转角速度。最后将线段的进入速度、最大进入速度限制、最大转角速度限制、正常运行速度、加速度、线段长度信息存入block_buffer环形队列中。

2.2、流程分析

2.2.1、圆弧插补

​ 圆弧插补，把圆弧拆分成多条逼近的直线段，然后对直线段进行插补，这种方法也就是俗称的把复杂曲线拆分成多条逼近的直线的插补方法。在plan_arc()函数中实现。

1、$获取圆心O：(x,y)起始点X_1：(x_1,x_2)终止点X_2:(x_2,y_2)$

ab_float_t rvec = -offset; //center_position=current_position+offset 圆心到起始点的向量OPs
 //Ps为当前点，O点坐标为(Ps.x+offset.x,Ps.y+offset.y)，则向量OPs=（-offset.x,-offset.y）=-offset。

2、计算终点始点夹角$\theta$

由向量点积和叉积公式

$$\vec{a}\cdot \vec{b}=|\vec{a}||\vec{b}|\ast cos(\theta )\text{\hspace{1em}(2-1)}$$

$$|\vec{a}\times \vec{b}|=|\vec{a}||\vec{b}|\ast sin(\theta )\text{\hspace{1em}(2-2)}$$

联立（2-1）（2-2）可以得到θ得反正切函数，从而计算出θ的值

$$y=x_2{y}_1-y_2{x}_1\text{\hspace{1em}(2-3)}$$

$$x=x_1{x}_2+y_1{y}_2\text{\hspace{1em}(2-4)}$$

$$\theta =atan2(y,x)\text{\hspace{1em}(2-5)}$$

angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y); //这里用到了向量点积和叉积公式

3、计算圆弧半径：

$$r=|\overrightarrow{O{x}_1}|\text{\hspace{1em}(2-6)}$$
弧长：

$$L=r\theta \text{\hspace{1em}(2-7)}$$
插补段数segments:
$$\frac{总弧长}{小线段长}\text{\hspace{1em}(2-8)}$$

 const float radius = HYPOT(rvec.a, rvec.b),            //计算圆弧半径radius
      		  center_P = current_position[axis_p] - rvec.a, //圆心坐标，center_P=ps.x+offset.x,center_Q=ps.y+offset.y
              center_Q = current_position[axis_q] - rvec.b,
              rt_X = cart[axis_p] - center_P,                //计算圆弧终点向量OPe，OPe=Pe-O
              rt_Y = cart[axis_q] - center_Q;
// rvec 圆心到起始点的向量OPs，rt_X,rt_Y,圆心到终点的向量OPe

 //根据设置获得移动的理想分段长度
const float ideal_segment_mm = (
    #if ARC_SEGMENTS_PER_SEC  // 基于分段每秒和进料速率的长度 
      constrain(scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC), MIN_ARC_SEGMENT_MM, MAX_ARC_SEGMENT_MM)
    #else
      MAX_ARC_SEGMENT_MM      //  使用最大段大小的长度
    #endif
  );
  //  nominal_segments基于理想段长度的整段数  nominal_segment_mm 基于理想整段数的线段长度
  const float nominal_segments = _MAX(FLOOR(flat_mm / ideal_segment_mm), min_segments), 
              nominal_segment_mm = flat_mm / nominal_segments;
  const uint16_t segments = nominal_segment_mm > (MAX_ARC_SEGMENT_MM) ? CEIL(flat_mm / (MAX_ARC_SEGMENT_MM)) :
                            nominal_segment_mm < (MIN_ARC_SEGMENT_MM) ? _MAX(1, FLOOR(flat_mm / (MIN_ARC_SEGMENT_MM))) :
                            nominal_segments;  //插补段数
  const float segment_mm = flat_mm / segments;  //每段小线段的长度

4、计算圆弧每个分段的夹角

因为夹角T不断变化，因此可以使用泰勒展开公式将其展开成幂级数形式计算。

$$sin(x)=x-\frac{1}{3!}{x}^3+\frac{1}{5!}{x}^5-\frac{1}{7!}{x}^7+...\text{\hspace{1em}(2-9)}$$

$$cos(x)=x-\frac{1}{2!}{x}^2+\frac{1}{4!}{x}^4-\frac{1}{6!}{x}^6+...\text{\hspace{1em}(2-10)}$$

由三角函数泰勒展开公式可得：

$$sin(T)=T-\frac{T^3}{6}\text{\hspace{1em}(2-11)}$$

$$cos(T)=1-\frac{T^2}{2}\text{\hspace{1em}(2-12)}$$

计算过程代码如下：

const float theta_per_segment = angular_travel / segments,  //每段的角度
                sq_theta_per_segment = sq(theta_per_segment),  //sq(x) = x^2
                sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,  //sinx泰勒三阶展开
                cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation //cos(x)二阶展开

//第二种计算方式，半径矢量的圆弧校正。只计算每一个N_ARC_CORRECTION增量。
const float Ti = i * theta_per_segment, cos_Ti = cos(Ti), sin_Ti = sin(Ti);   //计算OPi，OPi=(rcos(θs+θi),rsin(θs+θi)),θi=i*theta_per_segment

6、根据极坐标和夹角T求下一段线段始点坐标

由极坐标可得：

$$x=rcos(x)y=rsin(x)\text{\hspace{1em}(2-13)}$$
即始点坐标为：$(rcos(x),rsin(x))$

根据夹角T可以得到下一段起始点$(x_{new},y_{new})$:

$$(rcos(x+T),rsin(x+T))\text{\hspace{1em}(2-14)}$$

$$\begin{gathered} x_{new}=a.x\ast cos(T)-a.y\ast sin(T) \\ {y}_{new}=a.y\ast cos(T)+a.x\ast sin(T)\text{\hspace{1em}(2-15)} \end{gathered}$$

const float r_new_Y = rvec.a * sin_T + rvec.b * cos_T;  //计算下一个线段起始点Y坐标 rsin(a+T) =  rsin(a)cos(T)+rcos(a)sin(T) = y*cos(T)+ x*sin(T)
          rvec.a = rvec.a * cos_T - rvec.b * sin_T;   //下一线段起始点X坐标 rcos(a+T) = rsin(a)cos(T) - rsin(a)sin(T) = x*cos(T) - ysin(T)
          rvec.b = r_new_Y;
// Update raw location  //OPi的坐标=圆心坐标+OPi的坐标
      raw[axis_p] = center_P + rvec.a;
      raw[axis_q] = center_Q + rvec.b;

7、目标坐标输入，进行直线插补

2.1.3、速度预处理

​ 直线规划的实现函数在planner.cpp的Planner::buffer_line函数，buffer_line函数又调用buffer_segment函数中的buffer_steps，buffer_steps首先调用populate_block()函数生成新的规划block并进行填充，填充时调用了转角平滑算法来计算进入速度，然后再调用recalculate()函数来执行速度前瞻算法。生成规划block，block中有进入速度、退出速度、加速度、加速距离、减速距离等参数。

1、计算各轴的插补距离

$$target(目标位置/step)-position(当前位置/step)\text{\hspace{1em}(2-16)}$$

int32_t LOGICAL_AXIS_LIST(
    de = target.e - position.e,
    da = target.a - position.a,
    db = target.b - position.b,
    dc = target.c - position.c,
    di = target.i - position.i,
    dj = target.j - position.j,
    dk = target.k - position.k,
    du = target.u - position.u,
    dv = target.v - position.v,
    dw = target.w - position.w
    );
//block->steps[idx]是各轴当前位置到目标位置之间的插补距离
block->steps.set(NUM_AXIS_LIST(
    #if CORE_IS_XY                   //不同结构
      ABS(da + db), ABS(da - db), ABS(dc)
    #elif CORE_IS_XZ                           
      ABS(da + dc), ABS(db), ABS(da - dc)
    #elif CORE_IS_YZ
      ABS(da), ABS(db + dc), ABS(db - dc)
    #elif ENABLED(MARKFORGED_XY)
      ABS(da + db), ABS(db), ABS(dc)
    #elif ENABLED(MARKFORGED_YX)
      ABS(da), ABS(db + da), ABS(dc)
    #elif IS_SCARA
      ABS(da), ABS(db), ABS(dc)
    #else // default non-h-bot planning
      ABS(da), ABS(db), ABS(dc)
    #endif
    , ABS(di), ABS(dj), ABS(dk), ABS(du), ABS(dv), ABS(dw)
  ));

2、判断插补方向

 // Compute direction bit-mask for this block 计算该块的方向位掩码
  axis_bits_t dm = 0;
  #if ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX)
    if (da < 0) SBI(dm, X_HEAD);                  // Save the toolhead's true direction in X
    if (db < 0) SBI(dm, Y_HEAD);                  // ...and Y
    TERN_(HAS_Z_AXIS, if (dc < 0) SBI(dm, Z_AXIS));
  #endif
  #if IS_CORE
    #if CORE_IS_XY
      if (da + db < 0) SBI(dm, A_AXIS);           // Motor A direction
      if (CORESIGN(da - db) < 0) SBI(dm, B_AXIS); // Motor B direction
    #elif CORE_IS_XZ
      if (da < 0) SBI(dm, X_HEAD);                // Save the toolhead's true direction in X
      if (db < 0) SBI(dm, Y_AXIS);
      if (dc < 0) SBI(dm, Z_HEAD);                // ...and Z
      if (da + dc < 0) SBI(dm, A_AXIS);           // Motor A direction
      if (CORESIGN(da - dc) < 0) SBI(dm, C_AXIS); // Motor C direction
    #elif CORE_IS_YZ
      if (da < 0) SBI(dm, X_AXIS);
      if (db < 0) SBI(dm, Y_HEAD);                // Save the toolhead's true direction in Y
      if (dc < 0) SBI(dm, Z_HEAD);                // ...and Z
      if (db + dc < 0) SBI(dm, B_AXIS);           // Motor B direction
      if (CORESIGN(db - dc) < 0) SBI(dm, C_AXIS); // Motor C direction
    #endif
  #elif ENABLED(MARKFORGED_XY)
    if (da + db < 0) SBI(dm, A_AXIS);              // Motor A direction
    if (db < 0) SBI(dm, B_AXIS);                   // Motor B direction
  #elif ENABLED(MARKFORGED_YX)
    if (da < 0) SBI(dm, A_AXIS);                   // Motor A direction
    if (db + da < 0) SBI(dm, B_AXIS);              // Motor B direction
  #else
    XYZ_CODE(
      if (da < 0) SBI(dm, X_AXIS),
      if (db < 0) SBI(dm, Y_AXIS),
      if (dc < 0) SBI(dm, Z_AXIS)
    );
  #endif

  SECONDARY_AXIS_CODE(
    if (di < 0) SBI(dm, I_AXIS),
    if (dj < 0) SBI(dm, J_AXIS),
    if (dk < 0) SBI(dm, K_AXIS),
    if (du < 0) SBI(dm, U_AXIS),
    if (dv < 0) SBI(dm, V_AXIS),
    if (dw < 0) SBI(dm, W_AXIS)
  );

  #if HAS_EXTRUDERS
    if (de < 0) SBI(dm, E_AXIS);
    const float esteps_float = de * e_factor[extruder];
    const uint32_t esteps = ABS(esteps_float) + 0.5f;
  #else
    constexpr uint32_t esteps = 0;
  #endif

  // Clear all flags, including the "busy" bit
  block->flag.clear();
  block->direction_bits = dm;               // Set direction bits 设置插补方向

3、计算各轴插补步数

block->steps.set(NUM_AXIS_LIST(
    #if CORE_IS_XY
      ABS(da + db), ABS(da - db), ABS(dc)
    #elif CORE_IS_XZ                           //Ender3 pro 龙门架
      ABS(da + dc), ABS(db), ABS(da - dc)
    #elif CORE_IS_YZ
      ABS(da), ABS(db + dc), ABS(db - dc)
    #elif ENABLED(MARKFORGED_XY)
      ABS(da + db), ABS(db), ABS(dc)
    #elif ENABLED(MARKFORGED_YX)
      ABS(da), ABS(db + da), ABS(dc)
    #elif IS_SCARA
      ABS(da), ABS(db), ABS(dc)
    #else // default non-h-bot planning
      ABS(da), ABS(db), ABS(dc)
    #endif
    , ABS(di), ABS(dj), ABS(dk), ABS(du), ABS(dv), ABS(dw)
  ));

4、计算移动的空间距离s

由空间坐标系距离公式可得：
$$s^2=x^2+y^2+z^2\text{\hspace{1em}(2-17)}$$

 float distance_sqr = (
        #if ENABLED(ARTICULATED_ROBOT_ARM
          NUM_AXIS_GANG(
              sq(steps_dist_mm.x), + sq(steps_dist_mm.y), + sq(steps_dist_mm.z),
            + sq(steps_dist_mm.i), + sq(steps_dist_mm.j), + sq(steps_dist_mm.k),
            + sq(steps_dist_mm.u), + sq(steps_dist_mm.v), + sq(steps_dist_mm.w)
          )
        #elif ENABLED(FOAMCUTTER_XYUV)
          #if HAS_J_AXIS
            // Special 5 axis kinematics. Return the largest distance move from either X/Y or I/J plane
            _MAX(sq(steps_dist_mm.x) + sq(steps_dist_mm.y), sq(steps_dist_mm.i) + sq(steps_dist_mm.j))
          #else // Foamcutter with only two axes (XY)
            sq(steps_dist_mm.x) + sq(steps_dist_mm.y)
          #endif
        #elif ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX)
          XYZ_GANG(sq(steps_dist_mm.head.x), + sq(steps_dist_mm.head.y), + sq(steps_dist_mm.z))
        #elif CORE_IS_XZ
          XYZ_GANG(sq(steps_dist_mm.head.x), + sq(steps_dist_mm.y),      + sq(steps_dist_mm.head.z))
        #elif CORE_IS_YZ
          XYZ_GANG(sq(steps_dist_mm.x),      + sq(steps_dist_mm.head.y), + sq(steps_dist_mm.head.z))
        #else
          XYZ_GANG(sq(steps_dist_mm.x),       + sq(steps_dist_mm.y),      + sq(steps_dist_mm.z))
        #endif
      );

block->millimeters = SQRT(distance_sqr);  // 两点之间的距离s

5、计算额定速度和确定加速度

根据$v=\frac{S}{t}$， 具体代码如下：

//fr_mm_s 目标移动速度（形参）
//block->millimeters 距离S
//inverse_millimeters 距离倒数1/s
  const float inverse_millimeters = 1.0f / block->millimeters; 
  float inverse_secs;   //inverse_secs 时间倒数1/t
  #if BOTH(HAS_ROTATIONAL_AXES, INCH_MODE_SUPPORT)
    inverse_secs = inverse_millimeters * (cartesian_move ? fr_mm_s : LINEAR_UNIT(fr_mm_s));
  #else
    inverse_secs = fr_mm_s * inverse_millimeters;   //求出时间的倒数1/t=v/s 
  #endif
  const uint8_t moves_queued = nonbusy_movesplanned();
  // 当缓冲区开始清空时减慢速度，而不是在角落等待缓冲区重新填充
  #if EITHER(SLOWDOWN, HAS_WIRED_LCD) || defined(XY_FREQUENCY_LIMIT)
    //以微秒为单位分割时间
    int32_t segment_time_us = LROUND(1000000.0f / inverse_secs);                #endif
  #if ENABLED(SLOWDOWN)
    #ifndef SLOWDOWN_DIVISOR
      #define SLOWDOWN_DIVISOR 2
    #endif
    if (WITHIN(moves_queued, 2, (BLOCK_BUFFER_SIZE) / (SLOWDOWN_DIVISOR) - 1)) {
      const int32_t time_diff = settings.min_segment_time_us - segment_time_us;
      if (time_diff > 0) {
        //缓冲区正在耗尽，所以要增加额外的时间。如果缓冲区仍然被更多地清空，则添加的时间会增加。
        const int32_t nst = segment_time_us + LROUND(2 * time_diff / moves_queued);
        inverse_secs = 1000000.0f / nst;
        #if defined(XY_FREQUENCY_LIMIT) || HAS_WIRED_LCD
          segment_time_us = nst;
        #endif
      }
    }
  #endif
  #if HAS_WIRED_LCD
    // 保护进入该位置的通道。
    const bool was_enabled = stepper.suspend();
    block_buffer_runtime_us += segment_time_us;
    block->segment_time_us = segment_time_us;
    if (was_enabled) stepper.wake_up();
  #endif
//v = s/t，求出额定速度平方nominal_speed_sqr，即梯形曲线的匀速阶段的速度，单位 mm/sec 
  block->nominal_speed = block->millimeters * inverse_secs; 
//额定速率nominal_rate，这个block的单位为steps/sec的额定速度 
  block->nominal_rate = CEIL(block->step_event_count * inverse_secs); 
  #if ENABLED(FILAMENT_WIDTH_SENSOR)
    if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM)   // Only for extruder with filament sensor
      filwidth.advance_e(steps_dist_mm.e);
  #endif

  //以毫米/秒(线性)或度/秒(旋转)计算和限制速度
  xyze_float_t current_speed;  //xyze各轴当前速度
  float speed_factor = 1.0f; // factor <1 降低速度
  // 线性轴优先，逻辑较少
  LOOP_NUM_AXES(i) {
    current_speed[i] = steps_dist_mm[i] * inverse_secs; //每个轴当前的速度s/t
    const feedRate_t cs = ABS(current_speed[i]),
                 max_fr = settings.max_feedrate_mm_s[i]; //max_feedrate_mm_s每个轴电机最大移动速度
    if (cs > max_fr) NOMORE(speed_factor, max_fr / cs); //NOMORE:if (speed_factor > max_fr/cs) speed_factor = max_fr/cs
  }
  }
  // 如果有，则限制挤出机速度，
  #if HAS_EXTRUDERS
    {
      current_speed.e = steps_dist_mm.e * inverse_secs;
      #if HAS_MIXER_SYNC_CHANNEL
        //以指定的速率移动所有混合挤出机
        if (mixer.get_current_vtool() == MIXER_AUTORETRACT_TOOL)
          current_speed.e *= MIXING_STEPPERS;
      #endif
      const feedRate_t cs = ABS(current_speed.e),
                   max_fr = settings.max_feedrate_mm_s[E_AXIS_N(extruder)]
                           * TERN(HAS_MIXER_SYNC_CHANNEL, MIXING_STEPPERS, 1);
      if (cs > max_fr) NOMORE(speed_factor, max_fr / cs); //尊重任何运动的最大进给率（无论是否仅 E 轴）
      
      #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
        const feedRate_t max_vfr = volumetric_extruder_feedrate_limit[extruder]
                                   * TERN(HAS_MIXER_SYNC_CHANNEL, MIXING_STEPPERS, 1);
 		// TODO: Doesn't work properly for joined segments. Set MIN_STEPS_PER_SEGMENT 1 as workaround.
        // TODO：对于连接的段不能正常工作。将 MIN_STEPS_PER_SEGMENT 1 设置为解决方法。
        if (block->steps.a || block->steps.b || block->steps.c) {

          if (max_vfr > 0 && cs > max_vfr) {
            NOMORE(speed_factor, max_vfr / cs); //遵守挤出机体积限制（如果有）
          }
        }
      #endif
    }
  #endif

  #ifdef XY_FREQUENCY_LIMIT
    static axis_bits_t old_direction_bits; // = 0
    if (xy_freq_limit_hz) {
      // 检查并限制xy方向变化频率
      const axis_bits_t direction_change = block->direction_bits ^ old_direction_bits;
      old_direction_bits = block->direction_bits;
      segment_time_us = LROUND(float(segment_time_us) / speed_factor);

      static int32_t xs0, xs1, xs2, ys0, ys1, ys2;
      if (segment_time_us > xy_freq_min_interval_us)
        xs2 = xs1 = ys2 = ys1 = xy_freq_min_interval_us;
      else {
        xs2 = xs1; xs1 = xs0;
        ys2 = ys1; ys1 = ys0;
      }
      xs0 = TEST(direction_change, X_AXIS) ? segment_time_us : xy_freq_min_interval_us;
      ys0 = TEST(direction_change, Y_AXIS) ? segment_time_us : xy_freq_min_interval_us;

      if (segment_time_us < xy_freq_min_interval_us) {
        const int32_t least_xy_segment_time = _MIN(_MAX(xs0, xs1, xs2), _MAX(ys0, ys1, ys2));
        if (least_xy_segment_time < xy_freq_min_interval_us) {
          float freq_xy_feedrate = (speed_factor * least_xy_segment_time) / xy_freq_min_interval_us;
          NOLESS(freq_xy_feedrate, xy_freq_min_speed_factor);
          NOMORE(speed_factor, freq_xy_feedrate);
        }
      }
    }

  #endif // XY_FREQUENCY_LIMIT

  // Correct the speed  速度修正，根据speed_factor的值
  if (speed_factor < 1.0f) {
    current_speed *= speed_factor;    //各轴当前速度
    block->nominal_rate *= speed_factor;  //额定速率
    block->nominal_speed *= speed_factor;   //额定速度
  }

加速度设置，需要根据挤出机的状态选择不同加速度计算方式 ，部分代码如下：

 //为梯形速度控制算法计算加速度并限幅
  //step_event_count 完成这个block所需走的步数，steps_x, steps_y, steps_z, steps_e的最大值
  const float steps_per_mm = block->step_event_count * inverse_millimeters;  //单位转换 mm/s^2-step/s^2
  uint32_t accel;
  #if ENABLED(LIN_ADVANCE)
    bool use_advance_lead = false;
  #endif
  if (NUM_AXIS_GANG(
         !block->steps.a, && !block->steps.b, && !block->steps.c,
      && !block->steps.i, && !block->steps.j, && !block->steps.k,
      && !block->steps.u, && !block->steps.v, && !block->steps.w)
  ) {                                                      
    accel = CEIL(settings.retract_acceleration * steps_per_mm);   //设置加速度 (step/s^2)
  }
  else {
    #define LIMIT_ACCEL_LONG(AXIS,INDX) do{ \
      if (block->steps[AXIS] && max_acceleration_steps_per_s2[AXIS+INDX] < accel) { \
        const uint32_t max_possible = max_acceleration_steps_per_s2[AXIS+INDX] * block->step_event_count / block->steps[AXIS]; \
        NOMORE(accel, max_possible); \
      } \
    }while(0)
    #define LIMIT_ACCEL_FLOAT(AXIS,INDX) do{ \
      if (block->steps[AXIS] && max_acceleration_steps_per_s2[AXIS+INDX] < accel) { \
        const float max_possible = float(max_acceleration_steps_per_s2[AXIS+INDX]) * float(block->step_event_count) / float(block->steps[AXIS]); \
        NOMORE(accel, max_possible); \
      } \
    }while(0)
    -
    // ------------------默认加速度或挤出加速度设置 -------------------         
    accel = CEIL((esteps ? settings.acceleration : settings.travel_acceleration) * steps_per_mm);    //判断设置为默认加速度或者是挤出加速度
      #if ENABLED(LIN_ADVANCE)
      // HAS_I_AXIS 线性前进当前尚未准备好
      #define MAX_E_JERK(N) TERN(HAS_LINEAR_E_JERK, max_e_jerk[E_INDEX_N(N)], max_jerk.e
      /**
       * 如果所有这些都为真，则对块使用 LIN_ADVANCE
       * esteps                       : 这是一个打印移动，因为我们之前检查了a, B, C步。
       * extruder_advance_K[extruder] : 为该挤出机设置了一个提前因子。
       * de > 0                       : 挤出机向前运行(例如，“在收缩时擦拭”(Slic3r)或“梳理”(Cura)移动)
       */
      use_advance_lead = esteps && extruder_advance_K[E_INDEX_N(extruder)] && de > 0;

      if (use_advance_lead) {
        float e_D_ratio = (target_float.e - position_float.e) /
          TERN(IS_KINEMATIC, block->millimeters,
            SQRT(sq(target_float.x - position_float.x)
               + sq(target_float.y - position_float.y)
               + sq(target_float.z - position_float.z))
          );

        // 检查是否存在异常高的 e_D 比率，以检测由于最小值而导致的回缩移动是否与最后的打印移动相结合。每段的步骤。切勿提前执行此操作！
        //这假设没有人会使用 0mm < retr_length < ~0.2mm 的缩回长度，并且没有人会使用 3mm 耗材打印 100mm 宽的线条或使用 1.75mm 耗材打印 35mm 宽的线条。
        if (e_D_ratio > 3.0f)
          use_advance_lead = false;
        else {
          // Scale E acceleration so that it will be possible to jump to the advance speed.
          const uint32_t max_accel_steps_per_s2 = MAX_E_JERK(extruder) / (extruder_advance_K[E_INDEX_N(extruder)] * e_D_ratio) * steps_per_mm;
          if (TERN0(LA_DEBUG, accel > max_accel_steps_per_s2))
            SERIAL_ECHOLNPGM("Acceleration limited.");
          NOMORE(accel, max_accel_steps_per_s2);
        }
      }
    #endif

    //  每轴极限加速度
    if (block->step_event_count <= acceleration_long_cutoff) {
      LOGICAL_AXIS_CODE(
        LIMIT_ACCEL_LONG(E_AXIS, E_INDEX_N(extruder)),
        LIMIT_ACCEL_LONG(A_AXIS, 0), LIMIT_ACCEL_LONG(B_AXIS, 0), LIMIT_ACCEL_LONG(C_AXIS, 0),
        LIMIT_ACCEL_LONG(I_AXIS, 0), LIMIT_ACCEL_LONG(J_AXIS, 0), LIMIT_ACCEL_LONG(K_AXIS, 0),
        LIMIT_ACCEL_LONG(U_AXIS, 0), LIMIT_ACCEL_LONG(V_AXIS, 0), LIMIT_ACCEL_LONG(W_AXIS, 0)
      );
    }
    else {
      LOGICAL_AXIS_CODE(
        LIMIT_ACCEL_FLOAT(E_AXIS, E_INDEX_N(extruder)),
        LIMIT_ACCEL_FLOAT(A_AXIS, 0), LIMIT_ACCEL_FLOAT(B_AXIS, 0), LIMIT_ACCEL_FLOAT(C_AXIS, 0),
        LIMIT_ACCEL_FLOAT(I_AXIS, 0), LIMIT_ACCEL_FLOAT(J_AXIS, 0), LIMIT_ACCEL_FLOAT(K_AXIS, 0),
        LIMIT_ACCEL_FLOAT(U_AXIS, 0), LIMIT_ACCEL_FLOAT(V_AXIS, 0), LIMIT_ACCEL_FLOAT(W_AXIS, 0)
      );
    }
  }
  block->acceleration_steps_per_s2 = accel;                                                             //加速度 该块的标称步进速率，单位为step/s^2
  block->acceleration = accel / steps_per_mm;                                                           //加速度 该块的标称速度(mm/s^2)
  #if DISABLED(S_CURVE_ACCELERATION)
    block->acceleration_rate = (uint32_t)(accel * (float(1UL << 24) / (STEPPER_TIMER_RATE)));
  #endif
  #if ENABLED(LIN_ADVANCE)
    block->la_advance_rate = 0;
    block->la_scaling = 0;

    if (use_advance_lead) {
      // Bresenham 算法会将此步速转换为挤出机步数
      block->la_advance_rate = extruder_advance_K[E_INDEX_N(extruder)] * block->acceleration_steps_per_s2;

      // reduce LA ISR frequency by calling it only often enough to ensure that there will
      // never be more than four extruder steps per call
      for (uint32_t dividend = block->steps.e << 1; dividend <= (block->step_event_count >> 2); dividend <<= 1)
        block->la_scaling++;

      #if ENABLED(LA_DEBUG)
        if (block->la_advance_rate >> block->la_scaling > 10000)
          SERIAL_ECHOLNPGM("eISR running at > 10kHz: ", block->la_advance_rate);
      #endif
    }
  #endif

6、计算最大转角速度

①最小偏差法

将链接速度当作圆周运动的速度，根据圆周运动原理计算出最大的转角速度。

由圆周运动可知，角速度$w$，$v=Rw$，$a=R{w}^2$

得出：$v_{mj}=\sqrt{aR}$

根据上述结论，因为a为已知，所以可以由半径R计算出最大的转角速。根据两线段的夹角$\theta$和半径$R$的关系可以求出圆周运动半径$R$的值
$$sin(\frac{\theta }{2})=\sqrt{\frac{1-cos(\theta )}{2}}=\frac{R}{\delta +R}cos(\theta )=\frac{\overrightarrow{OP}\cdot \overrightarrow{OQ}}{|\overrightarrow{OP}||\overrightarrow{OQ}|}\text{\hspace{1em}(2-18)}$$

$$R=\delta \ast \frac{sin(\frac{\theta }{2})}{1-sin(\frac{\theta }{2})}\text{\hspace{1em}(2-19)}$$

由公式2-17求出的距离S可得：

线段的单位向量为：
$$\overrightarrow{OP}=(\frac{x}{s},\frac{y}{s},\frac{z}{s})\text{\hspace{1em}(2-20)}$$

联立公式（2-19）和（2-20）可以得出：
$$v_{mj}=\sqrt{a\ast \delta \ast \frac{sin(\frac{\theta }{2})}{1-sin(\frac{\theta }{2})}}\text{\hspace{1em}(2-21)}$$

计算部分代码如下：

const float inverse_millimeters = 1.0f / block->millimeters; //1/s
xyze_float_t unit_vec =
      #if HAS_DIST_MM_ARG
        cart_dist_mm
      #else
        LOGICAL_AXIS_ARRAY(steps_dist_mm.e,
          steps_dist_mm.x, steps_dist_mm.y, steps_dist_mm.z,
          steps_dist_mm.i, steps_dist_mm.j, steps_dist_mm.k,
          steps_dist_mm.u, steps_dist_mm.v, steps_dist_mm.w)
      #endif
    ;
 unit_vec *= inverse_millimeters;  //求出当前线段单位向量 unit_vec={x/s,y/s,z/s}

//prev_unit_vec是上一段线段的单位向量，将unit_vec与-prev_unit_vec做点积就求出线段夹角余弦值cosθ
      float junction_cos_theta = LOGICAL_AXIS_GANG(
                                 + (-prev_unit_vec.e * unit_vec.e),
                                 + (-prev_unit_vec.x * unit_vec.x),
                                 + (-prev_unit_vec.y * unit_vec.y),
                                 + (-prev_unit_vec.z * unit_vec.z),
                                 + (-prev_unit_vec.i * unit_vec.i),
                                 + (-prev_unit_vec.j * unit_vec.j),
                                 + (-prev_unit_vec.k * unit_vec.k),
                                 + (-prev_unit_vec.u * unit_vec.u),
                                 + (-prev_unit_vec.v * unit_vec.v),
                                 + (-prev_unit_vec.w * unit_vec.w)
                               );
      if (junction_cos_theta > 0.999999f) {
        // 对于 0 度锐角路口，只需设置最小路口速度。
        vmax_junction_sqr = sq(float(MINIMUM_PLANNER_SPEED));  //转角速度V^2
      }
      else {
        //将 delta 向量转换为单位向量
        xyze_float_t junction_unit_vec = unit_vec - prev_unit_vec;
        normalize_junction_vector(junction_unit_vec);

        const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec);

        if (TERN0(HINTS_CURVE_RADIUS, hints.curve_radius)) {
          TERN_(HINTS_CURVE_RADIUS, vmax_junction_sqr = junction_acceleration * hints.curve_radius);
        }
        else {
          NOLESS(junction_cos_theta, -0.999999f); //检查数字四舍五入以避免被零除。
          const float sin_theta_d2 = SQRT(0.5f * (1.0f - junction_cos_theta)); //这里求sin(θ/2)
const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec); //加速度
vmax_junction_sqr = junction_acceleration * junction_deviation_mm * sin_theta_d2 / (1.0f - sin_theta_d2);          //应用转角公式计算最大转角速度 v^2 = a*r = a*δ*(sin(θ/2)/(1-sin(θ/2)))

②经典jerk

经典jerk原理为在到达拐角处前，电机X轴电机从$v_x$下降至拐角速度$jerk$，当到达拐角点时，电机速度又从$jerk$瞬间降到0，然后Y轴电机瞬间从0加速到jerk，最后在慢慢加速到$V_y$。因为在拐角处是连续以$jerk$速度进行拐弯，所以较大的$jerk$速度就会产生异常震动和丢步，较小则会使挤出机停留较长时间，导致耗材堆积。
$$saf{e}_{speed}=saf{e}_{speed}\ast \frac{mjerk}{jerk}\text{\hspace{1em}(2-22)}$$

 //------------------------jerk----------------------------//
  #if HAS_CLASSIC_JERK
    // 退出速度受到前一个最后一段完全停止的急加速的限制
    static float previous_safe_speed;
    // 以一个安全的速度开始(从这个速度开始机器可以立即停止)。jerk的核心
    float safe_speed = block->nominal_speed;
    #ifndef TRAVEL_EXTRA_XYJERK
      #define TRAVEL_EXTRA_XYJERK 0
    #endif
    const float extra_xyjerk = TERN0(HAS_EXTRUDERS, de <= 0) ? TRAVEL_EXTRA_XYJERK : 0;
    uint8_t limited = 0; //限制标志
    TERN(HAS_LINEAR_E_JERK, LOOP_NUM_AXES, LOOP_LOGICAL_AXES)(i) {
      const float jerk = ABS(current_speed[i]),   // 初始化jerk 从0开始，这个轴的速度变化
                  maxj = (max_jerk[i] + (i == X_AXIS || i == Y_AXIS ? extra_xyjerk : 0.0f)); // maxj  这个轴的最大jerk速度设置（参数）单位mm/s
      if (jerk > maxj) {                          // cs > mj :此时速度过快
        if (limited) {                            // 已限制
          const float mjerk = block->nominal_speed * maxj; // ns*mj，v*mj，v^2
          if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk; // v^2/v = v  适当减小速度达到maxj
        }
        else {          //jerk < maxj
          safe_speed *= maxj / jerk;              // Initial limit: ns*mj/cs  根据适当比例提高安全速度到maxj
          ++limited;                              // Initially limited
        }
      }
    }
    float vmax_junction;
//jerk速度修正
    if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) {
      // 估计两个连续段的连接处允许的最大速度。
      // 如果允许的最大速度低于进入/退出安全速度的最小值，
      // 那么机器不再滑行并且应使用安全进入/退出速度。
      // 乘以先前/当前标称速度的因子以获得按分量限制的速度。
      float v_factor = 1;
      limited = 0;
      //链接点速度将在连续的段之间共享。将结速度限制为最小值。
      // 选择较小的标称速度。滑行期间不得在路口达到更高的速度。
      float smaller_speed_factor = 1.0f;
      if (block->nominal_speed < previous_nominal_speed) {
        vmax_junction = block->nominal_speed;
        smaller_speed_factor = vmax_junction / previous_nominal_speed;
      }
      else
        vmax_junction = previous_nominal_speed;
      // 现在限制所有轴上的jerk速度
      TERN(HAS_LINEAR_E_JERK, LOOP_NUM_AXES, LOOP_LOGICAL_AXES)(axis) {
        //限制轴。我们必须区分：滑行、轴反转、完全停止.
        float v_exit = previous_speed[axis] * smaller_speed_factor,  //退出速度
              v_entry = current_speed[axis];   //进入速度
        if (limited) {   //修正速度
          v_exit *= v_factor;  
          v_entry *= v_factor;
        }
        //根据轴是否在同一方向上滑行或反转来计算加速度。
        const float jerk = (v_exit > v_entry)?
          //         coasting axis reversal 滑行轴反转
              ( (v_entry > 0 || v_exit < 0) ? (v_exit - v_entry) : _MAX(v_exit, -v_entry) )
            : // v_exit <= v_entry  
              ( (v_entry < 0 || v_exit > 0) ? (v_entry - v_exit) : _MAX(-v_exit, v_entry) );
        const float maxj = (max_jerk[axis] + (axis == X_AXIS || axis == Y_AXIS ? extra_xyjerk : 0.0f));
        if (jerk > maxj) {
          v_factor *= maxj / jerk; //修正系数
          ++limited;
        }
      }
      if (limited) vmax_junction *= v_factor;
      //现在过渡速度已知，这在考虑加加速度因素的同时最大化共享的退出/进入速度，应用单独的安全退出/进入速度将实现更快的打印是可能的。
      const float vmax_junction_threshold = vmax_junction * 0.99f;
      if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold)
        vmax_junction = safe_speed;
    }
    else
      vmax_junction = safe_speed;   //设置最大的转角速度为jerk速度即safe_speed
    previous_safe_speed = safe_speed;
    #if HAS_JUNCTION_DEVIATION
      NOMORE(vmax_junction_sqr, sq(vmax_junction));   // 将油门降至最大速度
    #else
      vmax_junction_sqr = sq(vmax_junction);     //平方
    #endif
  #endif // Classic Jerk Limiting

4、设置最大初速度为转角速度

vmax_junction_sqr = _MIN(vmax_junction_sqr, sq(block->nominal_speed), sq(previous_nominal_speed));
block->max_entry_speed_sqr = vmax_junction_sqr;             //设置最大进入速度为最大转角速度
block->entry_speed_sqr = sq(float(MINIMUM_PLANNER_SPEED));  //设置进入速度

2.1.4、速度前瞻

​ 连续执行多条线段插补的时候，为了加快轴的移动速度，执行完一条直线指令后不能停下来，然后重新启动执行下一条直线指令。而是需要保持一定的速度去执行下一条直线插补，但是由于相邻两条直线之间有一定的夹角，导致转弯的时候，轴的速度不能过快，而且考虑两条直线执行的最大速度限制和直线头尾速度衔接等问题，速度前瞻算法就是每个区块的退出速度等于前一个区块的进入速度，并且每个区块的退出速度与进入速度满足关系:Ve^2-V02<=2as

1、正向加速阶段(起点->终点)，进入速度为$v_{f0，i-1}$,加速度$a_{i-1}$,距离$S_{i-1}$

第i-1段退出速度为：

$$v_{ft,i-1}=\sqrt{v_{f0，i-1}^2+2a_i{S}_i}\text{\hspace{1em}(2-23)}$$
实际退出速度为：

$$v_{ft,i-1}=min(v_{mj,i},v_{ft,i-1})\text{\hspace{1em}(2-24)}$$
其中默认第一段直线进入速度为0

2、反向加速阶段(终点->起点)，第i段直线退出速度为$v_{bt,i}$，加速度为$a_i$，距离$S_i$

第i段进入速度为：

$$v_{b0,i}=\sqrt{v_{bt，i}^2+2a_i{S}_i}\text{\hspace{1em}(2-25)}$$
实际进入速度：
$$v_{b0,i}=min(v_{mj,i},v_{b0,i})\text{\hspace{1em}(2-26)}$$
其中默认第N段（最后一段）退出速度为0

3、叠加两个过程，得出第i段线段的进入、退出速度：

$$v_{0,i}=min(v_{f0,i},v_{b0,i})\text{\hspace{1em}(2-27)}$$

$$v_{t,i}=min(v_{ft,i},v_{bt,i})\text{\hspace{1em}(2-28)}$$

实现函数在planner_recalculate()中reverse_pass()、forward_pass()。

2.1.2、直线插补(Bresenham)（脉冲执行时）

1、输入线段的两个端点坐标：$x_1,y_1,x_2,y_2$

2、设置象素坐标初值：$x=x_1,y=y_1$

3、设置初始误差判别值：$p=2\ast (\Delta y-\Delta x)$

4、分别计算：$\Delta x=x_2-x_1、\Delta y=y_2-y_1$

5、循环确定下一线段坐标：

for(x=x1；x<=x2；x++)
　　　{ putpixel(x,y) ；
　　　　if(p>=0)
　　　　　{ y=y+1；
　　　　　　p=p+2*(dy-dx)；
　　　　　}
　　　　else
　　　　　{ p=p+2*dy；
　　　　　}
　　　}

6、根据Bresenham原理实现多轴同步

选取x,y,z绝对值最大的作为累加溢出值：$c=|max(x,y,z)|$

累加初值：$b=\frac{c}{2}$

三个轴输出脉冲的算法如下：

m=l=k=b;
for(i=0;i<c;i++)
{
    m+=x;
    l+=y;
    k+=z;
    if(m>=c)
    {
        x轴输出一个脉冲；
        m-=c;
    }        
    if(l>=c)
    {
        y轴输出一个脉冲；
        l-=c;
    }
    if(k>=c)
    {
       z轴输出一个脉冲；
       k-=c;
    }