開源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,若s<s1，則此時是加速段，速度遞增；若s>sk，則是減速段，速度遞減；否則，是勻速段，速度等於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<t<1

其中：

            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
      .......
  }
  
}