开源cnc项目Marlin2.0运动控制部分代码理解

本文主要梳理Marlin2.0工程代码中关于运动控制部分的理解。Marlin1.0工程代码用C语言写的，阅读起来比较容易。Marlin1.0主要核心算法包括圆弧插补、速度前瞻、转角速度圆滑、梯形速度规划、Bresenham多轴插补。Marlin2.0工程相对于Marlin1.0工程程序用了更多C++的写法，程序写的相对专业（晦涩），许多人不太适应，其实2.0比1.0主要是增加了S形速度规划。

1 程序主循环 、G代码解析、圆弧插补

程序主循环非常简洁：

void loop() {

  for (;;) {

    idle(); // Do an idle first so boot is slightly faster

    #if ENABLED(SDSUPPORT)
      card.checkautostart();
      if (card.flag.abort_sd_printing) abortSDPrinting();
    #endif

    queue.advance();

    endstops.event_handler();
  }
}

对上位机传过来的G代码解析都在queue.advance()函数中。G0、G1是直线插补命令，G3、G4是圆弧插补命令。源码路径中motion文件夹中G0_G1.cpp的G0_G1()就是解析G0、G1直线插补命令，G2_G3.cpp的G2_G3()就是解析圆弧插补命令。这里看圆弧插补函数

void plan_arc(

  const xyze_pos_t &cart,   // Destination position //目标位置

  const ab_float_t &offset, // Center of rotation relative to current_position 

                                         //相对于当前位current_position的圆心位置，有center_position=current_position+offset

  const uint8_t clockwise   // Clockwise? //顺时针还是逆时针插补

)

先列出圆弧插补原理示意图：

圆心坐标O(xc,yc)，起始点Ps(x1,y1),终点Pe(x2,y2)，起始点也是当前点。圆弧插补思想就是算出OPs与OPe的夹角θ，进而求出PsPe段圆弧长度L=rθ，程序设定圆弧插补精度为p，则插补段数为N=L/p，则可以求出第i段的角度为θi=θ1+θ*i/N，则Pi.x=PO.x+r*cos（θs+θi）=PO.x+rcosθscosθi-rsinθssinθi=PO.x+ps.x*cosθi-Ps.y*sinθi，Pi.y=PO.x+r*sin（θs+θi）=PO.x+rsinθscosθi+rcosθssinθi=PO.x+ps.y*cosθi+Ps.x*sinθi，则从Ps到Pe的圆弧插补可以等效于从Ps经一系列中间点P1,P2,.....Pn再到Pe的一系列直线插补。

讲完原理，再来分析代码。

ab_float_t rvec = -offset; //Ps为当前点，O点坐标为(Ps.x+offset.x,Ps.y+offset.y)，则向量OPs=（-offset.x,-offset.y）=-offset。

const float radius = HYPOT(rvec.a, rvec.b),         //计算弧长r,rvec.x=rcosθs,rvec.y=rsinθs

              #if ENABLED(AUTO_BED_LEVELING_UBL)

                start_L  = current_position[l_axis],

              #endif

              center_P = current_position[p_axis] - rvec.a,     //圆心坐标，center_P=ps.x+offset.x,center_Q=ps.y+offset.y

              center_Q = current_position[q_axis] - rvec.b,

              rt_X = cart[p_axis] - center_P,     //计算圆弧终点向量OPe，OPe=Pe-O

              rt_Y = cart[q_axis] - center_Q,

              linear_travel = cart[l_axis] - current_position[l_axis],

              extruder_travel = cart.e - current_position.e;
// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
  float angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y);//这里用到了向量点积和叉积公式，OPs.OPe=|OPs|*|OPe|*cosθ=OPs.x*OPe.y+OPs.y*OPe.x,OPs X OPe=|OPs|*|OPe|*sinθ=OPs.x*OPe.y-OPs.y*OPe.x
  if (angular_travel < 0) angular_travel += RADIANS(360);
  #ifdef MIN_ARC_SEGMENTS
    uint16_t min_segments = CEIL((MIN_ARC_SEGMENTS) * (angular_travel / RADIANS(360)));
    NOLESS(min_segments, 1U);
  #else
    constexpr uint16_t min_segments = 1;
  #endif
  if (clockwise) angular_travel -= RADIANS(360);

  // Make a circle if the angular rotation is 0 and the target is current position
  if (angular_travel == 0 && current_position[p_axis] == cart[p_axis] && current_position[q_axis] == cart[q_axis]) {
    angular_travel = RADIANS(360);
    #ifdef MIN_ARC_SEGMENTS
      min_segments = MIN_ARC_SEGMENTS;
    #endif
  }

   //求出弧长L=rθ,插补精度为MM_PER_ARC_SEGMENT，则插补总段数N=L/MM_PER_ARC_SEGMENT
  const float flat_mm = radius * angular_travel,
              mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : ABS(flat_mm);
  if (mm_of_travel < 0.001f) return;

  uint16_t segments = FLOOR(mm_of_travel / (MM_PER_ARC_SEGMENT));
  NOLESS(segments, min_segments);

将N个小圆弧当成直线进行插补：

  for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
........省略代码

      const float cos_Ti = cos(i * theta_per_segment), 
      sin_Ti = sin(i * theta_per_segment);
//计算OPi，OPi=(rcos(θs+θi),rsin(θs+θi)),θi=i*theta_per_segment
      rvec.a = -offset[0] * cos_Ti + offset[1] * sin_Ti;
      rvec.b = -offset[0] * sin_Ti - offset[1] * cos_Ti;
  
    // Update raw location //Pi的坐标=圆心坐标+OPi的坐标
    raw[p_axis] = center_P + rvec.a;
    raw[q_axis] = center_Q + rvec.b;
    #if ENABLED(AUTO_BED_LEVELING_UBL)
      raw[l_axis] = start_L;
      UNUSED(linear_per_segment);
    #else
      raw[l_axis] += linear_per_segment;
    #endif
    raw.e += extruder_per_segment;

    apply_motion_limits(raw);

    #if HAS_LEVELING && !PLANNER_LEVELING
      planner.apply_leveling(raw);
    #endif
    
    //开始执行直线插补，目标点raw
    if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, MM_PER_ARC_SEGMENT
      #if ENABLED(SCARA_FEEDRATE_SCALING)
        , inv_duration
      #endif
    ))
      break;
  }

2 直线规划及速度前瞻算法

直线规划的实现函数在planner.cpp的Planner::buffer_line函数，buffer_line函数又调用buffer_segment函数，

bool Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e
  #if IS_KINEMATIC && DISABLED(CLASSIC_JERK)
    , const xyze_float_t &delta_mm_cart
  #endif
  , const feedRate_t &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/
) {
    //调用_buffer_steps进行直线规划，主要是生成一个新的规划block，block中填充初速度、末速度、加速度、加速距离、减速距离等
    if (
    !_buffer_steps(target   
      #if HAS_POSITION_FLOAT
        , target_float
      #endif
      #if IS_KINEMATIC && DISABLED(CLASSIC_JERK)
        , delta_mm_cart
      #endif
      , fr_mm_s, extruder, millimeters
    )
  ) return false;

  stepper.wake_up();//直线规划完以后唤醒定时器中断，在中断里根据规划的block执行速度规划
  return true;
}

_buffer_steps首先调用_populate_block()函数生成新的规划block并进行填充，填充时调用了转角平滑算法来计算初速度，然后再调用recalculate()函数来执行速度前瞻算法和梯形轨迹规划算法。我们先分析_populate_block()函数。

_populate_block()函数

我们来看一下要生成的block结构：

typedef struct block_t {

  volatile uint8_t flag;                    // Block flags (See BlockFlag enum above) - Modified by ISR and main thread!

  // Fields used by the motion planner to manage acceleration
  float nominal_speed_sqr,                  // The nominal speed for this block in (mm/sec)^2
        entry_speed_sqr,                    // Entry speed at previous-current junction in (mm/sec)^2
        max_entry_speed_sqr,                // Maximum allowable junction entry speed in (mm/sec)^2
        millimeters,                        // The total travel of this block in mm
        acceleration;                       // acceleration mm/sec^2

  union {
    abce_ulong_t steps;                     // Step count along each axis
    abce_long_t position;                   // New position to force when this sync block is executed
  };
  uint32_t step_event_count;                // The number of step events required to complete this block

  #if EXTRUDERS > 1
    uint8_t extruder;                       // The extruder to move (if E move)
  #else
    static constexpr uint8_t extruder = 0;
  #endif

  #if ENABLED(MIXING_EXTRUDER)
    MIXER_BLOCK_FIELD;                      // Normalized color for the mixing steppers
  #endif

  // Settings for the trapezoid generator
  uint32_t accelerate_until,                // The index of the step event on which to stop acceleration
           decelerate_after;                // The index of the step event on which to start decelerating

  #if ENABLED(S_CURVE_ACCELERATION)
    uint32_t cruise_rate,                   // The actual cruise rate to use, between end of the acceleration phase and start of deceleration phase
             acceleration_time,             // Acceleration time and deceleration time in STEP timer counts
             deceleration_time,
             acceleration_time_inverse,     // Inverse of acceleration and deceleration periods, expressed as integer. Scale depends on CPU being used
             deceleration_time_inverse;
  #else
    uint32_t acceleration_rate;             // The acceleration rate used for acceleration calculation
  #endif

  uint8_t direction_bits;                   // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)

  // Advance extrusion
  #if ENABLED(LIN_ADVANCE)
    bool use_advance_lead;
    uint16_t advance_speed,                 // STEP timer value for extruder speed offset ISR
             max_adv_steps,                 // max. advance steps to get cruising speed pressure (not always nominal_speed!)
             final_adv_steps;               // advance steps due to exit speed
    float e_D_ratio;
  #endif

  uint32_t nominal_rate,                    // The nominal step rate for this block in step_events/sec
           initial_rate,                    // The jerk-adjusted step rate at start of block
           final_rate,                      // The minimal rate at exit
           acceleration_steps_per_s2;       // acceleration steps/sec^2

  #if HAS_CUTTER
    cutter_power_t cutter_power;            // Power level for Spindle, Laser, etc.
  #endif

  #if FAN_COUNT > 0
    uint8_t fan_speed[FAN_COUNT];
  #endif

  #if ENABLED(BARICUDA)
    uint8_t valve_pressure, e_to_p_pressure;
  #endif

  #if HAS_SPI_LCD
    uint32_t segment_time_us;
  #endif

  #if ENABLED(POWER_LOSS_RECOVERY)
    uint32_t sdpos;
  #endif

} block_t;

_populate_block函数就是根据要规划的直线参数生成一个新的规划区块并填充它（有点像区块链）。我们进入_populate_block函数：

/**
 * Planner::_populate_block
 *
 * Fills a new linear movement in the block (in terms of steps).
 *
 *  target      - target position in steps units
 *  fr_mm_s     - (target) speed of the move
 *  extruder    - target extruder
 *
 * Returns true is movement is acceptable, false otherwise
 */
bool Planner::_populate_block(block_t * const block, bool split_move,
  const abce_long_t &target
  #if HAS_POSITION_FLOAT
    , const xyze_pos_t &target_float
  #endif
  #if IS_KINEMATIC && DISABLED(CLASSIC_JERK)
    , const xyze_float_t &delta_mm_cart
  #endif
  , feedRate_t fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/
) {
const int32_t da = target.a - position.a,//position为上一个插补点的坐标，target-position为插补距离
              db = target.b - position.b,
              dc = target.c - position.c;
#if EXTRUDERS
    int32_t de = target.e - position.e;
#else
constexpr int32_t de = 0;
#endif

uint8_t dm = 0;
  #if CORE_IS_XY
    ......一大堆宏，看着好累
  #else
    if (da < 0) SBI(dm, X_AXIS);
    if (db < 0) SBI(dm, Y_AXIS);
    if (dc < 0) SBI(dm, Z_AXIS);
  #endif
  if (de < 0) SBI(dm, E_AXIS);
// Clear all flags, including the "busy" bit
  block->flag = 0x00;

  // Set direction bits //设置插补方向
  block->direction_bits = dm;
 
 .........
  

 //设置各轴插补步数
  block->steps.set(ABS(da), ABS(db), ABS(dc));

 .........
 //求出移动的距离s
  block->millimeters = SQRT(
        #if CORE_IS_XY
          sq(delta_mm.head.x) + sq(delta_mm.head.y) + sq(delta_mm.z)
        #elif CORE_IS_XZ
          sq(delta_mm.head.x) + sq(delta_mm.y) + sq(delta_mm.head.z)
        #elif CORE_IS_YZ
          sq(delta_mm.x) + sq(delta_mm.head.y) + sq(delta_mm.head.z)
        #else
          sq(delta_mm.x) + sq(delta_mm.y) + sq(delta_mm.z)
        #endif
      );
  
  //step_event_count设置为各轴最大移动步数
  block->step_event_count = _MAX(block->steps.a, block->steps.b, block->steps.c, esteps);
  
  .......
  //求出距离倒数1/s
  const float inverse_millimeters = 1.0f / block->millimeters;  // Inverse millimeters to     
  remove multiple divides

  float inverse_secs = fr_mm_s * inverse_millimeters;//求出时间的倒数1/t=v/s
  
  ......
  //求出额定速度平方nominal_speed_sqr和额定速率nominal_rate
  block->nominal_speed_sqr = sq(block->millimeters * inverse_secs);   // (mm/sec)^2 Always > 0
  block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0

 .......
 //下面这段是设置加速度
 // Start with print or travel acceleration
 accel = CEIL((esteps ? settings.acceleration : settings.travel_acceleration) * steps_per_mm);
 ......
 block->acceleration_steps_per_s2 = accel;
 block->acceleration = accel / steps_per_mm;
 
 ........
 //开始转角速度平方
 float vmax_junction_sqr;
 #if DISABLED(CLASSIC_JERK)
    xyze_float_t unit_vec =
      #if IS_KINEMATIC && DISABLED(CLASSIC_JERK)
        delta_mm_cart
      #else
        { delta_mm.x, delta_mm.y, delta_mm.z, delta_mm.e }
      #endif
    ;
    unit_vec *= inverse_millimeters;//求出当前线段单位向量 unit_vec={x/s,y/s,z/s}
    ......
 // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
    if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
      // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
      // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
      //prev_unit_vec是上一段线段的单位向量，将unit_vec与-prev_unit_vec做点积就求出线段夹角余弦值cosθ
      float junction_cos_theta = (-prev_unit_vec.x * unit_vec.x) + (-prev_unit_vec.y * unit_vec.y)
                               + (-prev_unit_vec.z * unit_vec.z) + (-prev_unit_vec.e * unit_vec.e);

      // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
      if (junction_cos_theta > 0.999999f) {
        // For a 0 degree acute junction, just set minimum junction speed.
        vmax_junction_sqr = sq(float(MINIMUM_PLANNER_SPEED));
      }
      else {
        NOLESS(junction_cos_theta, -0.999999f); // Check for numerical round-off to avoid divide by zero.

        // Convert delta vector to unit vector
        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),
                    sin_theta_d2 = SQRT(0.5f * (1.0f - junction_cos_theta)); // Trig half angle identity. Always positive. 
//这里求sin(θ/2)
        //应用转角公式计算最大转角速度 v^2=a*r
        vmax_junction_sqr = (junction_acceleration * junction_deviation_mm * sin_theta_d2) / (1.0f - sin_theta_d2);

        if (block->millimeters < 1) {

          // Fast acos approximation, minus the error bar to be safe
          const float junction_theta = (RADIANS(-40) * sq(junction_cos_theta) - RADIANS(50)) * junction_cos_theta + RADIANS(90) - 0.18f;

          // If angle is greater than 135 degrees (octagon), find speed for approximate arc
          if (junction_theta > RADIANS(135)) {
            const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * junction_acceleration;
            NOMORE(vmax_junction_sqr, limit_sqr);
          }
        }
      }

      // Get the lowest speed
      vmax_junction_sqr = _MIN(vmax_junction_sqr, block->nominal_speed_sqr, previous_nominal_speed_sqr);
    }
    else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later.
      vmax_junction_sqr = 0;

    prev_unit_vec = unit_vec;
 #endif

 ........
 block->max_entry_speed_sqr = vmax_junction_sqr;//设置最大初速度为最大转角速度

  // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
  const float v_allowable_sqr = max_allowable_speed_sqr(-block->acceleration, sq(float(MINIMUM_PLANNER_SPEED)), block->millimeters);//求出允许的最大速度,v_allowable_sqr^2 =2as+MINIMUM_PLANNER_SPEED^2

  // If we are trying to add a split block, start with the
  // max. allowed speed to avoid an interrupted first move.
  block->entry_speed_sqr = !split_move ? sq(float(MINIMUM_PLANNER_SPEED)) : _MIN(vmax_junction_sqr, v_allowable_sqr);
.......
}

这里解释一下计算转角速度时的算法。如下图所示，P1P2与P2P3的夹角为θ，进而可求出求出sin(θ/2)=sqrt（(1-cosθ)/2），根据设置的弧长容差h，有：sin(θ/2)=r/(r+h)，进而可求出r=h*sin(θ/2)/(1-sin(θ/2))。有了r后可以由圆弧加速度公式：v*v=a*r，求出允许的最大转角速度v。

 

recalculate(）函数

recalculate(）函数代码：

void Planner::recalculate() {
  // Initialize block index to the last block in the planner buffer.
  const uint8_t block_index = prev_block_index(block_buffer_head);
  // If there is just one block, no planning can be done. Avoid it!
  if (block_index != block_buffer_planned) {
    reverse_pass();
    forward_pass();
  }
  recalculate_trapezoids();
}

速度前瞻算法在reverse_pass()和forward_pass(）函数中实现，速度规划在recalculate_trapezoids()函数中实现。速度前瞻算法就是从当前待执行的区块往后规划很多个区块，使得每个区块的末速度等于前一个区块的初速度，并且每个区块的末速度与初速度满足关系:Ve^2-V0^2<=2*a*s。

reverse_pass()从当前新产生的区块往后递推到最前一个没有被处理过的区块，使得前后俩个区块的初速度V(i)满足V(i)^2<=V(i+1)^2+2*a*s。V(i+1)为该区块下一个区块的初速度。forward_pass()函数从最后一个被处理过的模块往前递推到当前新加入的区块，使得前后俩个区块的末速度满足V(i)^2<=V(i-1)^2+2*a*s。reverse_pass()规划的是区块的初速度，forward_pass()规划的是区块的末速度。

3 梯形速度与S形速度曲线规划

recalculate(）中通过速度速度前瞻算法调整了各个区块以后，最后调用recalculate_trapezoids()执行速度曲线规划。该函数从当前已执行完的区块block_buffer_tail出开始往前到head_block_index，对之间的每个区块调用函数calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr)，执行速度曲线规划算法。速度曲线默认是梯形速度曲线，如果使能了S_CURVE_ACCELERATION，则执行S形曲线规划。

梯形速度规划

如左图所示，当采用梯形加速度法规划速度曲线时，初速度为0，末速度为t，正常运行速度为n。加速度段走过的距离s1=(n2-02)/2a，减速段曲线运行距离s3=(n2-t2)/2a，则恒速段走过的距离s2=s-s1-s2。若s2>0，则证明有恒速段，加速段和匀速段总距离sk为s1+s2。

当s2<0时没有恒速段时，曲线退化为只有加速和减速过程，如上边右图所示，设加速段和减速度段交点处速度为m，则有关系式：

当s1<0时，证明此时没有加速段只有减速段。当s1>s时，说明此时只有加速段没有减速段。由此可以求出加速段距离为s1，匀速段距离为0，则加速段和匀速段总距离sk=s1。这样在中断发波函数中，就可以根据已走过的距离s,若ssk，则是减速段，速度递减；否则，是匀速段，速度等于vn。(也可能没有匀速段)。

梯形速度规划函数calculate_trapezoid_for_block()的作用就是根据初速度v0，末速度vt，最大工作速度vn、加速度a和运动距离s规划计算出加速距离s1,以及开始减速距离sk=s1+s2（加速段加上匀速段，过后就是减速度了）。中断发波部分代码将在后文降到。

S形速度规划

Marlin2.0中使用的S形速度规划是基于梯形速度规划，先做好梯形速度规划，然后将加速段和减速段改成S形加减速曲线。用的S形曲线时6点Bezier曲线。

6点Bezier曲线是5阶形式：

V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) + P_4 * B_4(t) + P_5 * B_5(t)，0

其中：

            B_0(t) =   (1-t)^5        =   -t^5 +  5t^4 - 10t^3 + 10t^2 -  5t   +   1

            B_1(t) =  5(1-t)^4 * t    =   5t^5 - 20t^4 + 30t^3 - 20t^2 +  5t

            B_2(t) = 10(1-t)^3 * t^2  = -10t^5 + 30t^4 - 30t^3 + 10t^2

            B_3(t) = 10(1-t)^2 * t^3  =  10t^5 - 20t^4 + 10t^3

            B_4(t) =  5(1-t)   * t^4  =  -5t^5 +  5t^4

            B_5(t) =             t^5  =    t^5

V（t）可以改写为：

V(t) = A*t^5 + B*t^4 + C*t^3 + D*t^2 + E*t + F

其中：

          A =    -P_0 +  5*P_1 - 10*P_2 + 10*P_3 -  5*P_4 +  P_5

         B =   5*P_0 - 20*P_1 + 30*P_2 - 20*P_3 +  5*P_4

         C = -10*P_0 + 30*P_1 - 30*P_2 + 10*P_3

         D =  10*P_0 - 20*P_1 + 10*P_2

         E = - 5*P_0 +  5*P_1

         F =     P_0

我们希望初始加速度和初始jerk都为0，因此我们设置P_i=P_0 = P_1 = P_2 (initial velocity），P_t = P_3 = P_4 = P_5 (target velocity)，经过简化以后有：

         A = - 6*P_i +  6*P_t =  6*(P_t - P_i)

         B =  15*P_i - 15*P_t = 15*(P_i - P_t)

         C = -10*P_i + 10*P_t = 10*(P_t - P_i)

         D = 0

         E = 0

         F = P_i

此时有 V(t) = A*t^5 + B*t^4 + C*t^3 + F          [0 <= t <= 1]

calculate_trapezoid_for_block(）函数

void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) {
  
  //这里计算初速度和末速度
  uint32_t initial_rate = CEIL(block->nominal_rate * entry_factor),
           final_rate = CEIL(block->nominal_rate * exit_factor); // (steps per second)

  // Limit minimal step rate (Otherwise the timer will overflow.)
  NOLESS(initial_rate, uint32_t(MINIMAL_STEP_RATE));
  NOLESS(final_rate, uint32_t(MINIMAL_STEP_RATE));
  
  //定义S形曲线的最大速度cruise_rate 
  #if ENABLED(S_CURVE_ACCELERATION)
    uint32_t cruise_rate = initial_rate;
  #endif
  
  //取区块的加速度a
  const int32_t accel = block->acceleration_steps_per_s2;

 //这里先假设存在匀速段，匀速段速度vn，计算加速段s1和减速段s2
          // Steps required for acceleration, deceleration to/from nominal rate
  uint32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
           decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel));

//这里计算匀速段距离s3=s-s1-s2
          // Steps between acceleration and deceleration, if any
  int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;

  // Does accelerate_steps + decelerate_steps exceed step_event_count?
  // Then we can't possibly reach the nominal rate, there will be no cruising.
  // Use intersection_distance() to calculate accel / braking time in order to
  // reach the final_rate exactly at the end of this block.
  if (plateau_steps < 0) {//匀速段距离小于0，不存在匀速段，此时重新计算加速距离s1,和最大速度cruise_rate 
    const float accelerate_steps_float = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
    accelerate_steps = _MIN(uint32_t(_MAX(accelerate_steps_float, 0)), block->step_event_count);
    plateau_steps = 0;

    #if ENABLED(S_CURVE_ACCELERATION)
      // We won't reach the cruising rate. Let's calculate the speed we will reach
      cruise_rate = final_speed(initial_rate, accel, accelerate_steps);//计算最大
    #endif
  }

  #if ENABLED(S_CURVE_ACCELERATION)
    else // We have some plateau time, so the cruise rate will be the nominal rate
      cruise_rate = block->nominal_rate;//存在匀速段，最大速度等于vn
  #endif
  
  //如果使能S曲线，则计算加速时间、减速时间
  #if ENABLED(S_CURVE_ACCELERATION)
    // Jerk controlled speed requires to express speed versus time, NOT steps
    uint32_t acceleration_time = ((float)(cruise_rate - initial_rate) / accel) * (STEPPER_TIMER_RATE),
             deceleration_time = ((float)(cruise_rate - final_rate) / accel) * (STEPPER_TIMER_RATE);

    // And to offload calculations from the ISR, we also calculate the inverse of those times here
    uint32_t acceleration_time_inverse = get_period_inverse(acceleration_time);
    uint32_t deceleration_time_inverse = get_period_inverse(deceleration_time);
  #endif

  // Store new block parameters
  block->accelerate_until = accelerate_steps;
  block->decelerate_after = accelerate_steps + plateau_steps;
  block->initial_rate = initial_rate;
  #if ENABLED(S_CURVE_ACCELERATION)
    block->acceleration_time = acceleration_time;
    block->deceleration_time = deceleration_time;
    block->acceleration_time_inverse = acceleration_time_inverse;
    block->deceleration_time_inverse = deceleration_time_inverse;
    block->cruise_rate = cruise_rate;
  #endif
  block->final_rate = final_rate;
}

4 中断发波ISR

buffer_segment（）函数中，调用_buffer_steps()根据直线参数规划完区块以后，马上调用wake_up()函数开启定时器中断，召唤stepper.cpp中的isr()中断函数执行规划好的区块，最后发送电机驱动脉冲。中断ISR是最终的执行部分，它从区块队列中取出一个待执行区块，执行了Bresenham多轴插补算法，并且根据规划好的速度曲线生成对应的速度，进而生成对应的占空比取控制脉冲频率。中断函数isr()调用了stepper_pulse_phase_isr()执行插补，调用stepper_block_phase_isr(）执行区块速度规划。

stepper_pulse_phase_isr()

void Stepper::stepper_pulse_phase_isr() {
    ......
 //这里step_error初始在stepper_block_phase_isr()中刚取到新区块时,初始值为-step_event_count
 advance_dividend初始化为各轴待插补步数*2，delta_error[AXIS]加上步数advance_divided[AXIS]
若大于0，则这个轴脉冲
     #define PULSE_PREP(AXIS) do{ \
      delta_error[_AXIS(AXIS)] += advance_dividend[_AXIS(AXIS)]; \
      step_needed[_AXIS(AXIS)] = (delta_error[_AXIS(AXIS)] >= 0); \
      if (step_needed[_AXIS(AXIS)]) { \
        count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \
        delta_error[_AXIS(AXIS)] -= advance_divisor; \
      } \
    }while(0)

    // Start an active pulse, if Bresenham says so, and update position
    
    #define PULSE_START(AXIS) do{ \
      if (step_needed[_AXIS(AXIS)]) { \
        _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS), 0); \ 
      } \
    }while(0)

    // Stop an active pulse, if any, and adjust error term
    //发波，就是让对应的引脚翻转
    #define PULSE_STOP(AXIS) do { \
      if (step_needed[_AXIS(AXIS)]) { \
        _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS), 0); \
      } \
    }while(0)

    // Determine if pulses are needed
    #if HAS_X_STEP
      PULSE_PREP(X); 
    #endif
    #if HAS_Y_STEP
      PULSE_PREP(Y);
    #endif
    #if HAS_Z_STEP
      PULSE_PREP(Z);
    #endif
    ......
    // Pulse start
    #if HAS_X_STEP
      PULSE_START(X);
    #endif
    #if HAS_Y_STEP
      PULSE_START(Y);
    #endif
    #if HAS_Z_STEP
      PULSE_START(Z);
    #endif
}

stepper_block_phase_isr(）

uint32_t Stepper::stepper_block_phase_isr() {
   .......
  // If there is a current block
  if (current_block) {
    //step_events_completed >= step_event_count，区块执行完毕，将该区块从队列中删除
    // If current block is finished, reset pointer
    if (step_events_completed >= step_event_count) {
      #ifdef FILAMENT_RUNOUT_DISTANCE_MM
        runout.block_completed(current_block);
      #endif
      axis_did_move = 0;
      current_block = nullptr;
      planner.discard_current_block();
    }
    else {
      // Step events not completed yet...
      
      
      // Are we in acceleration phase ? 加速段
      if (step_events_completed <= accelerate_until) { // Calculate new timer value

        #if ENABLED(S_CURVE_ACCELERATION)
         //s形加速段，执行bezeier速度公式V(t) = A*t^5 + B*t^4 + C*t^3 + F
          // Get the next speed to use (Jerk limited!)
          uint32_t acc_step_rate =
            acceleration_time < current_block->acceleration_time
              ? _eval_bezier_curve(acceleration_time)
              : current_block->cruise_rate;
        #else   //梯形加速段，v=initial_rate+a*t
          acc_step_rate = STEP_MULTIPLY(acceleration_time, current_block->acceleration_rate) + current_block->initial_rate;
          NOMORE(acc_step_rate, current_block->nominal_rate);
        #endif

        // acc_step_rate is in steps/second

        // step_rate to timer interval and steps per stepper isr
        interval = calc_timer_interval(acc_step_rate, oversampling_factor, &steps_per_isr);
        acceleration_time += interval;

        #if ENABLED(LIN_ADVANCE)
          if (LA_use_advance_lead) {
            // Fire ISR if final adv_rate is reached
            if (LA_steps && LA_isr_rate != current_block->advance_speed) nextAdvanceISR = 0;
          }
          else if (LA_steps) nextAdvanceISR = 0;
        #endif // LIN_ADVANCE
      }
      // Are we in Deceleration phase ? 减速段
      else if (step_events_completed > decelerate_after) {
        uint32_t step_rate;

        #if ENABLED(S_CURVE_ACCELERATION)
          //s形减速段，执行bezier减速段公式
          // If this is the 1st time we process the 2nd half of the trapezoid...
          if (!bezier_2nd_half) {
            // Initialize the Bézier speed curve
            _calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse);
            bezier_2nd_half = true;
            // The first point starts at cruise rate. Just save evaluation of the Bézier curve
            step_rate = current_block->cruise_rate;
          }
          else {
            // Calculate the next speed to use
            step_rate = deceleration_time < current_block->deceleration_time
              ? _eval_bezier_curve(deceleration_time)
              : current_block->final_rate;
          }
        #else
          //梯形减速段，v=final_rate-a*t
          // Using the old trapezoidal control
          step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate);
          if (step_rate < acc_step_rate) { // Still decelerating?
            step_rate = acc_step_rate - step_rate;
            NOLESS(step_rate, current_block->final_rate);
          }
          else
            step_rate = current_block->final_rate;
        #endif

        // step_rate is in steps/second

        // step_rate to timer interval and steps per stepper isr
        interval = calc_timer_interval(step_rate, oversampling_factor, &steps_per_isr);
        deceleration_time += interval;

        #if ENABLED(LIN_ADVANCE)
          if (LA_use_advance_lead) {
            // Wake up eISR on first deceleration loop and fire ISR if final adv_rate is reached
            if (step_events_completed <= decelerate_after + steps_per_isr || (LA_steps && LA_isr_rate != current_block->advance_speed)) {
              nextAdvanceISR = 0;
              LA_isr_rate = current_block->advance_speed;
            }
          }
          else if (LA_steps) nextAdvanceISR = 0;
        #endif // LIN_ADVANCE
      }
      // We must be in cruise phase otherwise 匀速段
      else {

        #if ENABLED(LIN_ADVANCE)
          // If there are any esteps, fire the next advance_isr "now"
          if (LA_steps && LA_isr_rate != current_block->advance_speed) nextAdvanceISR = 0;
        #endif

        // Calculate the ticks_nominal for this nominal speed, if not done yet
        if (ticks_nominal < 0) {
          // step_rate to timer interval and loops for the nominal speed
          ticks_nominal = calc_timer_interval(current_block->nominal_rate, oversampling_factor, &steps_per_isr);
        }

        // The timer interval is just the nominal value for the nominal speed
        interval = ticks_nominal;
      }
    }
  }
  
  //从队列中取一个规划好的区块并准备执行
  if (!current_block) {
      .......
      // Based on the oversampling factor, do the calculations
      step_event_count = current_block->step_event_count << oversampling;

      // Initialize Bresenham delta errors to 1/2
      delta_error = -int32_t(step_event_count);
      
      //初始化Bresenham参数
      // Calculate Bresenham dividends and divisors
      advance_dividend = current_block->steps << 1;
      advance_divisor = step_event_count << 1;

      // No step events completed so far
      step_events_completed = 0;

      // Compute the acceleration and deceleration points
      accelerate_until = current_block->accelerate_until << oversampling;
      decelerate_after = current_block->decelerate_after << oversampling;
      .......
      //计算bezeier系数
      #if ENABLED(S_CURVE_ACCELERATION)
        // Initialize the Bézier speed curve
        _calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time_inverse);
        // We haven't started the 2nd half of the trapezoid
        bezier_2nd_half = false;
      #endif
      .......
  }
  
}