代码收藏家 GRBL算法初版学习笔记：深入解析STM32步进电机算法
提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档
GRBL源码中步进电机的算法学习笔记（STM32）
前言
GRBL源码中算法部分的学习是我在公司研发激光切割机（三轴）期间研究的一套关于步进电机驱动控制的常见算法，以下内容都以激光切割机来举例，话不多说直接上干货。（以下内容皆为个人理解，如有错误可以在评论区揪出，希望大家一同讨论，共同进步）
一、简看GRBL的工作流程
对于一台激光切割机来说，当你想要雕刻一个圆
第一步：（将指令或图片G代码通过串口发送给MCU）
首先从上位机LaserGRBL/lightburn软件解析想要雕刻的圆，然后上位机将解析后的数据通过串口发送给MCU，MCU判断串口收到的数据是G代码还是“$”指令（该部分对应的函数为： protocol_execute_line）。
在本次举例中，圆会被解析成G代码，所以MCU通过源码中的Gcode解释器（Gcode解释器在源码中对应的函数为：gc_execute_line），但如果是指令则通过源码system_execute_line这个函数进行处理
第二部：（将接受到的G代码进行分析，从而调用对应的算法函数进行处理）
注：本文暂时不讨论指令如何处理和G代码如何分析只讨论算法
当Gcode解释器解释到数据是圆的时候，将会先计算圆心的坐标，然后再调用mc_arc函数将圆拆分成一段段小的线段（算法部分也就是从mc_arc这个函数开始）
{
算法部分
1、mc_arc() —> 将圆拆分成小线段，并计算线段的起点与终点的坐标
2、mc_line() —> 将线段与线段之间的切换速度进行计算
3、planner_recalculate（） —> 基于切换速度和前瞻算法，设定线段的最佳起始速度和结束速度
4、st_prep_buffer（）—> 梯形算法，将速度换算成每一步需要的时间，最后将时间数据通过定时器，改变输出频率作用到步进电机
}
二、算法部分（所有算法解释内容目前只放在代码块中体现）
1.mc_arc
代码如下（示例）：
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)//计算圆弧被拆分后，每一个坐标的位置并传送给mc_line()
#endif
{
float center_axis0 = position[axis_0] + offset[axis_0];//position为当前位置坐标，offset为圆心相对于当前位置的偏移量
float center_axis1 = position[axis_1] + offset[axis_1];
float r_axis0 = -offset[axis_0]; //假设圆心为0时，当前位置的坐标 // Radius vector from center to current location
float r_axis1 = -offset[axis_1];
float rt_axis0 = target[axis_0] - center_axis0;//假设圆心为0时，终点位置的坐标
float rt_axis1 = target[axis_1] - center_axis1;
uint16_t segments;
float theta_per_segment;
float linear_per_segment;
float cos_T,sin_T;
float sin_Ti;
float cos_Ti;
float r_axisi;
uint16_t i;
uint8_t count = 0;
// CCW angle between position and target from circle center. Only one atan2() trig computation required.
// 计算圆弧的终点向量∠B - 起始向量∠A的夹角对应的夹角弧度数,参考函数arctan(y/x) = ∠A 和 tan（∠B - ∠A）= tan∠B - tan∠A / 1 + tan∠A * tan∠B的算法
//angular_travel = arctan（∠B - ∠A） = (Yb/Xb - Ya/Xa) / (1 + Ya/Xa * Yb/Xb) -- 上下同时 *XaXb
//angular_travel = (Yb*Xa - Ya*Xb) / (Xa*Xb + Ya*Yb)
//夹角弧度数 = 同心角大小映射到pi上
float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);//atan2的返回值是弧度
if (is_clockwise_arc)//顺时针 //判断是顺时针还是逆时针，①终点向量与x轴的弧度制减去起始向量与x轴的弧度制的差为angular_travel
{
//由①可知，如果圆弧顺时针移动，角度应该是负值，如果计算出的角度为正值，需要在计算出的角度基础上减去2*pi（pi为圆周率）
if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= 2*M_PI; }
}
else//逆时针
{
//由①可知，如果圆弧逆时针移动，角度应该是正值，如果计算出的角度为负值，需要在计算出的角度基础上加上2*pi（pi为圆周率）
if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += 2*M_PI; }
}
// NOTE: Segment end points are on the arc, which can lead to the arc diameter being smaller by up to
// (2x) settings.arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit
// is desired, i.e. least-squares, midpoint on arc, just change the mm_per_arc_segment calculation.
// For the intended uses of Grbl, this value shouldn't exceed 2000 for the strictest of cases.
//settings.arc_tolerance这个参数是用于计算每一个线段的长度。arc_tolerance越小分出来的线段越多，插补越精确
//segments = （弧长） / （每一小段的长度）；判断可以分成多少份。//本行的详细解释在代码块下方的②处
segments = floor(fabs(0.5*angular_travel*radius)/
sqrt(settings.arc_tolerance*(2*radius - settings.arc_tolerance)) );
if (segments) {
// Multiply inverse feed_rate to compensate for the fact that this movement is approximated
// by a number of discrete segments. The inverse feed_rate should be correct for the sum of
// all segments.
if (invert_feed_rate) { feed_rate *= segments; }
theta_per_segment = angular_travel/segments;//细分后，每一段对应的弧度制的大小
linear_per_segment = (target[axis_linear] - position[axis_linear])/segments; //axis_linear，除了圆弧平面之外的第三个轴，即与圆弧平面垂直的轴
// 三角函数的泰勒级数Computes: cos（theta_per_segment） = 1 - theta_per_segment^2/2, \
sin（theta_per_segment） = theta_per_segment - theta_per_segment^3/6)
cos_T = 2.0 - theta_per_segment*theta_per_segment;//每份弧度数对应的2*cos pi/x
sin_T = theta_per_segment*0.16666667*(cos_T + 4.0);//每份弧度数对应的sin pi/x
cos_T *= 0.5;//每份弧度数对应的cosθ
//cosθ = 1 - θ^2 / 2
//sinθ = θ - θ^3 / 6
for (i = 1; i<segments; i++)
{ // Increment (segments-1).
//下方为计算旋转 ②单位角弧度 后的终点坐标原理：
//r_axis0、r_axis1分别是圆点到起始坐标的x、y轴的偏移量
//所以 设“偏移前”的起始坐标为（r_axis0，r_axis1），且假设C为当前角度，T为②单位弧度，半径为r那么
//r_axis0 = r * cosC
//r_axis1 = r * sinC
//那么 设“偏移T°后”的起始坐标为（r_axis0'，r_axis1'）
//r_axis0' = r * cos(C + T) = r*cosC*cosT - r*sinC*sinT = r_axis0*cosT - r_axis1*sinT
//r_axis1' = r * sin(C + T) = r*sinC*cosT + r*cosC*sinT = r_axis1*cosT + r_axis0*sinT = r_axis0*sin_T + r_axis1*cos_T
if (count < N_ARC_CORRECTION)
{
// Apply vector rotation matrix. ~40 usec
//r_axisi是个中间装载值，起到一个暂存的作用
//cos_T和sin_T是圆弧被细分后的每份角弧度的sin和cos值，②设每份角弧度也叫单位弧度
//r_axis0、r_axis1分别是当前起始坐标相对于圆心在x、y轴的偏移量
r_axisi = r_axis0*sin_T + r_axis1*cos_T;//计算当前坐标偏移单位角弧度后y轴的坐标，暂存于r_axisi
r_axis0 = r_axis0*cos_T - r_axis1*sin_T;//计算当前坐标偏移单位角弧度后x轴的坐标
r_axis1 = r_axisi;
count++;
}
else
{
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. ~375 usec
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
cos_Ti = cos(i*theta_per_segment);//计算第i个细分小线段的坐标于最开始的起始坐标的夹角的cos值
sin_Ti = sin(i*theta_per_segment);//计算第i个细分小线段的坐标于最开始的起始坐标的夹角的sin值
r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;//计算从最开始坐标偏移至当前坐标的总弧度后的x轴的坐标
r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;//计算从最开始坐标偏移至当前坐标的总弧度后的y轴的坐标
count = 0;
}
// Update arc_target location
//以上代码都是基于圆心为0，现在圆心为center_axis0，center_axis1
//总结一下三行就是将偏移后的真实坐标存放在position[axis_0]，position[axis_1]
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
// Bail mid-circle on system abort. Runtime command check already performed by mc_line.
//系统中止标志。 强制退出回到主循环以进行重置。
if (sys.abort) { return; }
}
}
// Ensure last segment arrives at target location.
#ifdef USE_LINE_NUMBERS
mc_line(target, feed_rate, invert_feed_rate, line_number);
#else
mc_line(target, feed_rate, invert_feed_rate);
#endif
②、上图的“弧ABC”就是“每一小段长度”对应的弧长，“每一小段的长度” 2 = 2AC2 = 2(OB2 – (OB – BD)2 )= 4OBBD – BD2
其中OB = radius，BD = arc_tolerance
所以segment（份数）= angular_travelradius)（总弧长）/ sqrt(settings.arc_tolerance*(2radius – settings.arc_tolerance))
{
//segments = （总弧长） / （每一小段的长度）；判断可以分成多少份。//本行的详细解释在代码块下方的②处
segments = floor(fabs(0.5angular_travelradius)/
sqrt(settings.arc_tolerance(2*radius – settings.arc_tolerance)) );
}
2.mc_line中的plan_buffer_line
代码如下（示例）：
void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) //拐点速度计算-->v^2 = a * r（即：线与线之间切换时的速度）
#endif
{
int32_t target_steps[N_AXIS];
float unit_vec[N_AXIS], delta_mm;
uint8_t idx;
float inverse_unit_vec_value;
float inverse_millimeters; // Inverse millimeters to remove multiple float divides
float junction_cos_theta;
float sin_theta_d2;
// Prepare and initialize new block
//尾插链表，先进先出队列逻辑
plan_block_t *block = &block_buffer[block_buffer_head];
block->step_event_count = 0;//该节点的两个轴中步数最大的那个轴的步数值
block->millimeters = 0;//当前节点的剩余执行距离
block->direction_bits = 0;//轴运动方向
block->acceleration = SOME_LARGE_VALUE;//稍后缩小至最大加速度 // Scaled down to maximum acceleration later
#ifdef USE_LINE_NUMBERS
block->line_number = line_number;
#endif
// Compute and store initial move distance data.
// TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea
// to try to keep these types of things completely separate from the planner for portability.
#ifdef COREX//settings.steps_per_mm每毫米行走的步数，即x step/mm
target_steps[A_MOTOR] = (target[A_MOTOR]*settings.steps_per_mm[A_MOTOR])>0?(int32_t)(target[A_MOTOR]*settings.steps_per_mm[A_MOTOR]+0.5):(int32_t)(target[A_MOTOR]*settings.steps_per_mm[A_MOTOR]-0.5);
//x轴电机要运行的步数，并四舍五入
target_steps[B_MOTOR] = (target[B_MOTOR]*settings.steps_per_mm[B_MOTOR])>0?(int32_t)(target[B_MOTOR]*settings.steps_per_mm[B_MOTOR]+0.5):(int32_t)(target[B_MOTOR]*settings.steps_per_mm[B_MOTOR]-0.5);
//y轴电机要运行的步数，并四舍五入
block->steps[A_MOTOR] = labs((target_steps[X_AXIS]-pl.position[X_AXIS]) + (target_steps[Y_AXIS]-pl.position[Y_AXIS]));
block->steps[B_MOTOR] = labs((target_steps[X_AXIS]-pl.position[X_AXIS]) - (target_steps[Y_AXIS]-pl.position[Y_AXIS]));
#endif
//N_AXIS即xyz三轴
for (idx=0; idx<N_AXIS; idx++) {//①
//target，为激光头期望移动到的坐标，坐标的单位为mm
//feed_rate，线段的最大运行速度
//steps_per_mm值，也就是每毫米代表的轴移动步数per/mm
target_steps[idx] = (target[idx]*settings.steps_per_mm[idx]) > 0 ? (int32_t)(target[idx]*settings.steps_per_mm[idx]+0.5) : (int32_t)(target[idx]*settings.steps_per_mm[idx]-0.5);
//电机从原点到目标点的各轴要执行的步数
block->steps[idx] = labs(target_steps[idx] - pl.position[idx]);//各个轴从当前坐标移动到期望坐标的步数
block->step_event_count = max(block->step_event_count, block->steps[idx]);//各个轴需要的移动步数中，取最高的放置在参数block->step_event_count中
delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx];//从当前坐标移动到期望坐标所需要移动的距离（单位: mm）
unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later.
// Set direction bits. Bit enabled always means direction is negative.
if (delta_mm < 0 )//如果Delta_mm为负数，那么就要反向行走
{
block->direction_bits |= get_direction_pin_mask(idx);
}
// Incrementally compute total move distance by Euclidean norm. First add square of each term.
block->millimeters += delta_mm*delta_mm;//每个轴平方的累加
}//①
block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt()
//将x、y轴要运行的距离的平方和再开根号，即三角行的第三边计算公式
// Bail if this is a zero-length block. Highly unlikely to occur.
if (block->step_event_count == 0) { return; } //如果最大要执行的步数为0，那么就直接退出函数
// Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids)
// TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort.
if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later//暂无
else if (invert_feed_rate) { feed_rate *= block->millimeters; }//暂无
if (feed_rate < MINIMUM_FEED_RATE) { feed_rate = MINIMUM_FEED_RATE; } // Prevents step generation round-off condition.//暂无
inverse_millimeters = 1.0/block->millimeters; // 取第三边的倒数Inverse millimeters to remove multiple float divides
junction_cos_theta = 0;
for (idx=0; idx<N_AXIS; idx++) {
/*主要公式 acc[x] = Acc * (x / (√x^2 + y^2))
acc[y] = Acc * (y / (√x^2 + y^2))
所以 Acc0 = acc[x] / (x / (√x^2 + y^2)) 、 Acc1 = acc[y] / (y / (√x^2 + y^2))
Acc = min(Acc0 ,Acc1);
*/
if (unit_vec[idx] != 0) { // Avoid divide by zero.
unit_vec[idx] *= inverse_millimeters; // Complete unit vector calculation //unit_vec[x] = x / (√x^2 + y^2)
inverse_unit_vec_value = fabs(1.0/unit_vec[idx]); // Inverse to remove multiple float divides. // inverse_unit_vec_value[x] = (√x^2 + y^2) / x
// Check and limit feed rate against max individual axis velocities and accelerations
feed_rate = min(feed_rate, settings.max_rate[idx]*inverse_unit_vec_value);//通过分量计算速度
block->acceleration = min(block->acceleration, settings.acceleration[idx]*inverse_unit_vec_value);//通过分量计算加速度
// Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction
// between the current move and the previous move is simply the dot product of the two unit vectors,
// where prev_unit_vec is negative. Used later to compute maximum junction speed.
junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx];
}
}//计算加速度Acc ，计算速度rate
// TODO: Need to check this method handling zero junction speeds when starting from rest.
if (block_buffer_head == block_buffer_tail) {
// Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later.
block->entry_speed_sqr = 0.0;
block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity.
} else {
// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
if (junction_cos_theta > 0.999999) {
// For a 0 degree acute junction, just set minimum junction speed.
block->max_junction_speed_sqr = MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED;
} else {
junction_cos_theta = max(junction_cos_theta,-0.999999); // Check for numerical round-off to avoid divide by zero.
sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive.
// TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the
// two junctions. However, this shouldn't be a significant problem except in extreme circumstances.
block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED,
(block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) );
}//计算出拐角速度的平方
}
// Store block nominal speed
block->nominal_speed_sqr = feed_rate*feed_rate; // 上位机传过来的期望速度的平方(mm/min). Always > 0
// Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds.
block->max_entry_speed_sqr = min(block->max_junction_speed_sqr,
min(block->nominal_speed_sqr, pl.previous_nominal_speed_sqr));//求出理论值的最大拐角速度
// Update previous path unit_vector and nominal speed (squared)
memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // 更新分量速度在pl.previous_unit_vec[]，pl.previous_unit_vec[] = unit_vec[]
pl.previous_nominal_speed_sqr = block->nominal_speed_sqr;//更新标称速度，用来给下一条线段进行对比
// Update planner position 更新激光头的坐标
memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[]
// New block is all set. Update buffer head and next buffer head indices.
//运行以下函数之前，假设当前节点block_buffer_head为head_node
block_buffer_head = next_buffer_head; //buffer_head指向当前节点head_node的下一个节点
next_buffer_head = plan_next_block_index(block_buffer_head);//buffer_next指向当前节点head_node的下一个节点的下一个节点
//以下函数planner_recalculate()会用到关于上诉节点改变后的节点，用来追溯最开始的head_node
// Finish up by recalculating the plan with the new block.
planner_recalculate();
3.planner_recalculate
代码如下（示例）：
static void planner_recalculate(void)//运动规划-》前瞻算法，和后顾算法
{
// Initialize block index to the last block in the planner buffer.
//在plan_buffer_line这个函数中提及到了block_buffer_head这个节点已经变为了自己的下一个节点
uint8_t block_index = plan_prev_block_index(block_buffer_head);//将block_index变成当前处理的节点的坐标
// Bail. Can't do anything with one only one plan-able block.
float entry_speed_sqr;
plan_block_t *next;
plan_block_t *current = &block_buffer[block_index];//current为当前节点
if (block_index == block_buffer_planned) { return; }
// Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
// block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
// NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
// Calculate maximum entry speed for last block in buffer, where the exit speed is always zero.
//current即是当前链表中最后一个处理的线段（也是最新处理的线段）现在设退出时的速度（即末速度）v1为0，那么v1^2 - v0^2 = 2ax可以推出进入是的初速度为2ax
current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters);
block_index = plan_prev_block_index(block_index);//将block_index变成当前线段的上一个线段的坐标
if (block_index == block_buffer_planned)// 当【Only two plannable blocks in buffer】. Reverse pass complete.
{
// Check if the first block is the tail. If so, notify stepper to update its current parameters.
if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); }
}
else // 当【Three or more plan-able blocks】
{
while (block_index != block_buffer_planned) //无t公式要理解清楚方向的定义！！！
{//这段代码的含义是从当前线段往前推，直到所有的线段都优化过退出循环，\
即每条线段的初速度不能超过线段设置的最大初速度的限制
next = current;
current = &block_buffer[block_index];
block_index = plan_prev_block_index(block_index);
// Check if next block is the tail block(=planned block). If so, update current stepper parameters.
if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); }
// Compute maximum entry speed decelerating over the current block from its exit speed.
if (current->entry_speed_sqr != current->max_entry_speed_sqr)
{
entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;//从最后一个块的退出速度当作初速度往前推（且最后一块的退出速度为0）
if (entry_speed_sqr < current->max_entry_speed_sqr)
current->entry_speed_sqr = entry_speed_sqr;
else
current->entry_speed_sqr = current->max_entry_speed_sqr;
}
}
}
// Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
// Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer
block_index = plan_next_block_index(block_buffer_planned);
while (block_index != block_buffer_head)
{ //这段代码的含义是从第一个没有优化过的线段往前，直到到达当前线段时退出循环，\
即每条线段的末速度不能超过下一条线段的初速度，这样多条线段才能保持连续的速度运行
current = next;
next = &block_buffer[block_index];
// Any acceleration detected in the forward pass automatically moves the optimal planned
// pointer forward, since everything before this is all optimal. In other words, nothing
// can improve the plan from the buffer tail to the planned pointer by logic.
if (current->entry_speed_sqr < next->entry_speed_sqr)
{
entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
// If true, current block is full-acceleration and we can move the planned pointer forward.
if (entry_speed_sqr < next->entry_speed_sqr)
{
next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
block_buffer_planned = block_index; // Set optimal plan pointer.
}
}
// Any block set at its maximum entry speed also creates an optimal plan up to this
// point in the buffer. When the plan is bracketed by either the beginning of the
// buffer and a maximum entry speed or two maximum entry speeds, every block in between
// cannot logically be further improved. Hence, we don't have to recompute them anymore.
if (next->entry_speed_sqr == next->max_entry_speed_sqr) { block_buffer_planned = block_index; }
block_index = plan_next_block_index( block_index );
}
}
4.st_prep_buffer
代码如下（示例）：
void st_prep_buffer(void)
{
float inv_2_accel;
float decel_dist;
float exit_speed_sqr;
float intersect_distance;
segment_t *prep_segment;
float dt_max; // Maximum segment time
float dt; // Initialize segment time
float time_var; // Time worker variable
float mm_var; // mm-Distance worker variable
float speed_var; // Speed worker variable
float mm_remaining; // New segment distance from end of block.
float minimum_mm; // Guarantee at least one step.
float steps_remaining; // Convert mm_remaining to steps
float n_steps_remaining; // Round-up current steps remaining
float last_n_steps_remaining; // Round-up last steps remaining
float inv_rate;
uint32_t cycles;
if (sys.state & (STATE_HOLD|STATE_MOTION_CANCEL|STATE_SAFETY_DOOR)) {
// Check if we still need to generate more segments for a motion suspend.
//检查我们是否仍需要为运动暂停生成更多段。
if (prep.current_speed == 0.0) { return; } // Nothing to do. Bail.
}
while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer.检查我们是否需要填充缓冲区。
// Determine if we need to load a new planner block or if the block has been replanned.
// 确定是否需要加载新的规划器块或者该块是否已被重新规划。
//判断当前线段拆分时间片是否完成，如果没有完成，pl_block不为空，if里的语句不会被执行。 \
如果pl_block为空，说明当前线段时间片拆分完成，执行if里的语句，开始把下一条线段的信息读出来进行时间片拆分
if (pl_block == NULL) {
pl_block = plan_get_current_block(); // Query planner for a queued block
if (pl_block == NULL) { return; } // No planner blocks. Exit.
// Check if the segment buffer completed the last planner block. If so, load the Bresenham
// data for the block. If not, we are still mid-block and the velocity profile was updated.
//检查段缓冲区是否完成了最后一个规划器块。 如果是，则加载该块的 Bresenham 数据。 \
如果没有，我们仍然处于中间状态并且速度剖面已更新。
if (prep.flag_partial_block) {//表示最后一个块已完成的标志。是时候装一个新的了。
prep.flag_partial_block = false; // Reset flag
} else {
// Increment stepper common data index to store new planner block data.
if ( ++prep.st_block_index == (SEGMENT_BUFFER_SIZE-1) ) { prep.st_block_index = 0; }
// Prepare and copy Bresenham algorithm segment data from the new planner block, so that
// when the segment buffer completes the planner block, it may be discarded when the
// segment buffer finishes the prepped block, but the stepper ISR is still executing it.
st_prep_block = &st_block_buffer[prep.st_block_index];//开辟新的队列st_block_buffer 存放新线段拆分时间片计算过程数据
st_prep_block->direction_bits = pl_block->direction_bits;//将运动方向赋值给st_prep_block->direction_bits
#ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS];
st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS];
st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS];
st_prep_block->step_event_count = pl_block->step_event_count;
#else
// With AMASS enabled, simply bit-shift multiply all Bresenham data by the max AMASS
// level, such that we never divide beyond the original data anywhere in the algorithm.
// If the original data is divided, we can lose a step from integer roundoff.
//AMASS功能用于平滑脉冲频率太慢的线段，如果AMASS功能使能，线段步数放大MAX_AMASS_LEVEL倍，\
但是定时器定时间隔将会缩短，相当于定时器中断加快了，\
更多次的中断累积才输出一个脉冲，这样输出脉冲变得更平滑了
st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS] << MAX_AMASS_LEVEL;
st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS] << MAX_AMASS_LEVEL;
st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS] << MAX_AMASS_LEVEL;
st_prep_block->step_event_count = pl_block->step_event_count << MAX_AMASS_LEVEL;
#endif
// Initialize segment buffer data for generating the segments.
prep.steps_remaining = pl_block->step_event_count; //将3轴之中运动最长的轴作为step_event_count，并且赋给steps_remaining
prep.step_per_mm = prep.steps_remaining/pl_block->millimeters;//换算成（x step/mm）,并赋值给step_per_mm
prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR/prep.step_per_mm;//1.25 / steep_per_mm(暂时不知道目的) -》一step能走多少mm 并 * 1.25
prep.dt_remainder = 0.0; // Reset for new planner block
/ zx /
if (sys.state & (STATE_HOLD|STATE_MOTION_CANCEL|STATE_SAFETY_DOOR)) {
// Override planner block entry speed and enforce deceleration during feed hold.
//覆盖规划程序块的进入速度并在进给保持期间强制减速。
prep.current_speed = prep.exit_speed;
pl_block->entry_speed_sqr = prep.exit_speed*prep.exit_speed;
}
else { prep.current_speed = sqrt(pl_block->entry_speed_sqr); } //sqrt开根号
/ zx /
}
/* ---------------------------------------------------------------------------------
Compute the velocity profile of a new planner block based on its entry and exit
speeds, or recompute the profile of a partially-completed planner block if the
planner has updated it. For a commanded forced-deceleration, such as from a feed
hold, override the planner velocities and decelerate to the target exit speed.
根据新规划器块的进入和退出速度计算其速度剖面，或者如果规划器已更新则重新计算部分完成的规划器块的剖面。
对于命令强制减速（例如进给保持），忽略规划器速度并减速到目标退出速度。
*/
prep.mm_complete = 0.0; // 默认速度曲线在距块末端 0.0mm 处完成。// Default velocity profile complete at 0.0mm from end of block.
inv_2_accel = 0.5/pl_block->acceleration; //inv_2_accel = (1 / 2a);方便个下面的加速度计算公式使用
if (sys.state & (STATE_HOLD|STATE_MOTION_CANCEL|STATE_SAFETY_DOOR)) { //[Forced Deceleration to Zero Velocity]//系统报错，xy轴速度强制为0
//该if满足条件的话，该步骤的目的是强制减速为0 //[Forced Deceleration to Zero Velocity]
// Compute velocity profile parameters for a feed hold in-progress. This profile overrides
// the planner block profile, enforcing a deceleration to zero speed.
// 计算正在进行的进给保持的速度曲线参数。 该配置文件覆盖规划器块配置文件，强制减速至零速度。
prep.ramp_type = RAMP_DECEL;//设置标志位为减速
// Compute decelerate distance relative to end of block.
decel_dist = pl_block->millimeters - inv_2_accel*pl_block->entry_speed_sqr; //判断现在剩下的距离pl_block->millimeters是否可以满足让当前速度pl_block->entry_speed_sqr减速到0 //zhaoxin
if (decel_dist < 0.0) {//此次运算不足以让速度减为0
// Deceleration through entire planner block. End of feed hold is not in this block.
prep.exit_speed = sqrt(pl_block->entry_speed_sqr-2*pl_block->acceleration*pl_block->millimeters);//计算退出时的速度 //zhaoxin
} else {
prep.mm_complete = decel_dist; // End of feed hold.
prep.exit_speed = 0.0;
}
} else { // [Normal Operation]正常操作
// Compute or recompute velocity profile parameters of the prepped planner block.
//计算或重新计算准备好的规划器块的速度剖面参数。
prep.ramp_type = RAMP_ACCEL; // Initialize as acceleration ramp.//设置标志位为加速
prep.accelerate_until = pl_block->millimeters; //zhaoxin
prep.exit_speed = plan_get_exec_block_exit_speed();//返回下一个块的进入速度，当做当前块的退出速度，具体数据可联系前瞻算法
exit_speed_sqr = prep.exit_speed*prep.exit_speed;//计算退出速度的平方
/*减速阶段距离intersect_distance的计算推导公式有三个
Vmax^2 - Vo^2 = 2aS1; //Vo为初速度，S1为加速阶段距离
Vmax^2 - Ve^2 = 2aS2; //Ve为末速度，S2为减速阶段距离
S1 + S2 = S总; //S总为当前线段的可移动距离
accelerate_until 和 decelerate_after这两个参数具体代表的意思为如下线段
maximum_speed (< nominal_speed) -> +
+--------+ <- maximum_speed (= nominal_speed) /|`
/ ` / | `
current_speed -> + ` / | + <- exit_speed
| + <- exit_speed / | |
+-------------+ current_speed -> +----+--+
time --> ^ ^ ^ ^
| | | |
decelerate_after(in mm) decelerate_after(in mm)
^ ^ ^ ^
| | | |
accelerate_until(in mm) accelerate_until(in mm)
*/
intersect_distance =
0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr));//
if (intersect_distance > 0.0)
{//说明改线段需要减速阶段，所以下面需要判断，将来处于哪一个阶段
if (intersect_distance < pl_block->millimeters)//剩余的距离大于所需要的减速距离，代表该if代表本次线段不止只有减速阶段
{ // Either trapezoid or triangle types //zhaoxin
//注意，以下计算对于加速状态或者匀速状态，该计算结果也为0// NOTE: For acceleration-cruise and cruise-only types, following calculation will be 0.0.
prep.decelerate_after = inv_2_accel*(pl_block->nominal_speed_sqr-exit_speed_sqr);//计算线段最大限制速度减速到末速度需要的减速距离
if (prep.decelerate_after < intersect_distance)//上位机设置的最大速度nominal_speed_sqr所需要的减速距离decelerate_after 小于 理论计算出的减速距离intersect_distance（）
{//以上条件满足，则代表理论需要的距离，满足 当前设置的最大速度nominal_speed_sqr减速为指定退出速度exit_speed_sqr所需要的距离intersect_distance
prep.maximum_speed = sqrt(pl_block->nominal_speed_sqr);
if (pl_block->entry_speed_sqr == pl_block->nominal_speed_sqr) { //如果初速度等于最大限制速度，那么线段只有匀速或者减速过程
// Cruise-deceleration or cruise-only type.
prep.ramp_type = RAMP_CRUISE;
} else {
// Full-trapezoid or acceleration-cruise types//具有加速，匀速和减速
prep.accelerate_until -= inv_2_accel*(pl_block->nominal_speed_sqr-pl_block->entry_speed_sqr); //总距离 减去 该线段加速时需使用的距离
}
} else { //减速距离大于交点距离，说明线段设置的最大限制速度大于交点处的最大速度，那么线段只有三角形状的加速和减速过程，没有匀速过程
prep.accelerate_until = intersect_distance;
prep.decelerate_after = intersect_distance;
prep.maximum_speed = sqrt(2.0*pl_block->acceleration*intersect_distance+exit_speed_sqr);
}
}
else { //剩余的距离不足以让速度减下来，则进入全减速状态// Deceleration-only type
prep.ramp_type = RAMP_DECEL;
// prep.decelerate_after = pl_block->millimeters;
prep.maximum_speed = prep.current_speed;//将当前速度设置为最大速度
}
} else { //减速距离为0，则代表仅设置为加速 // Acceleration-only type
prep.accelerate_until = 0.0;
// prep.decelerate_after = 0.0;
prep.maximum_speed = prep.exit_speed;
}
}
}
// Initialize new segment
prep_segment = &segment_buffer[segment_buffer_head];
// Set new segment to point to the current segment data block.
prep_segment->st_block_index = prep.st_block_index;
/*------------------------------------------------------------------------------------
Compute the average velocity of this new segment by determining the total distance
traveled over the segment time DT_SEGMENT. The following code first attempts to create
a full segment based on the current ramp conditions. If the segment time is incomplete
when terminating at a ramp state change, the code will continue to loop through the
progressing ramp states to fill the remaining segment execution time. However, if
an incomplete segment terminates at the end of the velocity profile, the segment is
considered completed despite having a truncated execution time less than DT_SEGMENT.
The velocity profile is always assumed to progress through the ramp sequence:
acceleration ramp, cruising state, and deceleration ramp. Each ramp's travel distance
may range from zero to the length of the block. Velocity profiles can end either at
the end of planner block (typical) or mid-block at the end of a forced deceleration,
such as from a feed hold.
通过确定段时间 DT_SEGMENT 内行驶的总距离来计算该新段的平均速度。
以下代码首先尝试根据当前斜坡条件创建完整段。
如果在斜坡状态更改时终止时段时间不完整，则代码将继续循环遍历正在进行的斜坡状态以填充剩余的段执行时间。
但是，如果不完整的段在速度曲线末尾终止，则尽管截断的执行时间小于 DT_SEGMENT，但该段仍被视为已完成。
始终假定速度曲线按照斜坡序列进行：加速斜坡、巡航状态和减速斜坡。
每个坡道的行驶距离范围可以从零到块的长度。
速度曲线可以在规划器块的末端（典型）结束，也可以在强制减速（例如进给保持）结束时的中间块结束。
*/
dt_max = DT_SEGMENT; // Maximum segment time
time_var = dt_max; // Time worker variable
dt=0.0; // Initialize segment time
mm_remaining = pl_block->millimeters; //新段距块末尾的距离。// New segment distance from end of block. //zhaoxin
minimum_mm = mm_remaining-prep.req_mm_increment; //保证至少一步。 // Guarantee at least one step.
if (minimum_mm < 0.0) { minimum_mm = 0.0; }
do {
switch (prep.ramp_type) {
case RAMP_ACCEL: //加速
// NOTE: Acceleration ramp only computes during first do-while loop.
speed_var = pl_block->acceleration*time_var;//单位时间内，加速度的变化值
mm_remaining -= time_var*(prep.current_speed + 0.5*speed_var);//总距离mm_remaining 减去 本次加速时使用的距离
//time_var*(prep.current_speed + 0.5*speed_var)的意思是：S = Vcur * t + 0.5 * Acc * t^2 ;(mm)
if (mm_remaining < prep.accelerate_until) { //当总距离mm_remaining不断减少，直到改线段加速斜坡完成时，进入if语句 // End of acceleration ramp.
// Acceleration-cruise, acceleration-deceleration ramp junction, or end of block.
mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB
time_var = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed);
/* 上诉公式的意义
Vc->当前速度, Vm->加速曲线的最终速度
Vc + Acc * time_var = Vm --》 time_var * Acc = Vm - Vc ①
Vm^2 - Vc^2 = 2*Acc*S --》 2*Acc*S = (Vm + Vc) * (Vm - Vc) ②
timer_var = 2 * S / (Vm + Vc) <- ① / ②
*/
//zhaoxin
if (mm_remaining == prep.decelerate_after) {//加速完成后剩下的总距离mm_remaining == 减速需要的距离decelerate_after，则代表本次线段有加速和减速（且加速已完成）
prep.ramp_type = RAMP_DECEL;
} else {//加速完成后剩下的总距离mm_remaining > 减速需要的距离decelerate_after则代表本次线段有加速、匀速和减速（且加速已完成）
prep.ramp_type = RAMP_CRUISE;
}
prep.current_speed = prep.maximum_speed;
} else { // Acceleration only.
prep.current_speed += speed_var;
}
break;
case RAMP_CRUISE: //匀速
// NOTE: mm_var used to retain the last mm_remaining for incomplete segment time_var calculations.
// NOTE: If maximum_speed*time_var value is too low, round-off can cause mm_var to not change. To
// prevent this, simply enforce a minimum speed threshold in the planner.
mm_var = mm_remaining - prep.maximum_speed*time_var;//当总距离mm_var不断减少，直到改线段匀速完成时，进入if语句
if (mm_var < prep.decelerate_after) {///当总距离mm_var不断减少，直到改线段匀速完成时，进入if语句 // End of cruise.
// Cruise-deceleration junction or end of block.
time_var = (mm_remaining - prep.decelerate_after)/prep.maximum_speed;//当前剩余的总距离 减去 减速阶段的距离 == 匀速阶段的距离（且只要有匀速阶段，那么就一定能达到最大速度maximum_speed）
mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB
prep.ramp_type = RAMP_DECEL;
} else { // Cruising only.
mm_remaining = mm_var;
}
break;
default: // case RAMP_DECEL: 减速
// NOTE: mm_var used as a misc worker variable to prevent errors when near zero speed.
speed_var = pl_block->acceleration*time_var; // Used as delta speed (mm/min)
if (prep.current_speed > speed_var) { // Check if at or below zero speed.
// Compute distance from end of segment to end of block.
mm_var = mm_remaining - time_var*(prep.current_speed - 0.5*speed_var);//减去本次循环在这个减速减速阶段所运行的距离 //time_var*(prep.current_speed - 0.5*speed_var)的意思是 = Vcur * t + 0.5 * (-Acc) * t^2 ;(mm)
if (mm_var > prep.mm_complete) { // Deceleration only.
mm_remaining = mm_var;
prep.current_speed -= speed_var;
break; // Segment complete. Exit switch-case statement. Continue do-while loop.
}
} // End of block or end of forced-deceleration.
time_var = 2.0*(mm_remaining-prep.mm_complete)/(prep.current_speed+prep.exit_speed);//计算这段减速所需要的时间
mm_remaining = prep.mm_complete;
}
dt += time_var; // Add computed ramp time to total segment time.
if (dt < dt_max) { time_var = dt_max - dt; } // **Incomplete** At ramp junction.
else {
if (mm_remaining > minimum_mm)
{ //检查非常慢的零步段。 Check for very slow segments with zero steps.
//增加分段时间以确保分段中至少前进一步。覆盖和循环 Increase segment time to ensure at least one step in segment. Override and loop
//通过距离计算，直到minimum_mm或mm_complete。 through distance calculations until minimum_mm or mm_complete.
dt_max += DT_SEGMENT;
time_var = dt_max - dt;
} else { //【经过一次阶段后剩下的总距离mm_remaining】 <= 【minimum_mm = 进入循环前的总距离millimeters - 每一步能移动的距离（且放大了1.25倍）】--》便可以退出循环
//即：【运动前的距离millimeters - 运动后的距离mm_remaining >= 每一步能移动的距离minimum_mm（且该距离minimum_mm放大了1.25倍)】便可以退出循环
break; // **Complete** Exit loop. Segment execution time maxed.
}
}
} while (mm_remaining > prep.mm_complete); // **Complete** Exit loop. Profile complete.
/* -----------------------------------------------------------------------------------
Compute segment step rate, steps to execute, and apply necessary rate corrections.
NOTE: Steps are computed by direct scalar conversion of the millimeter distance
remaining in the block, rather than incrementally tallying the steps executed per
segment. This helps in removing floating point round-off issues of several additions.
However, since floats have only 7.2 significant digits, long moves with extremely
high step counts can exceed the precision of floats, which can lead to lost steps.
Fortunately, this scenario is highly unlikely and unrealistic in CNC machines
supported by Grbl (i.e. exceeding 10 meters axis travel at 200 step/mm).
*/
steps_remaining = prep.step_per_mm*mm_remaining; //将运动后剩余的距离转换成步数// Convert mm_remaining to steps
n_steps_remaining = ceil(steps_remaining); //将运动后剩下的步数向上取整// Round-up current steps remaining
last_n_steps_remaining = ceil(prep.steps_remaining);//将整条线段中，运动步数最多的一个轴的步数向上取整 // Round-up last steps remaining
prep_segment->n_step = last_n_steps_remaining - n_steps_remaining; //总步数 减去 运动后剩余的步数 = 本次循环运动所需要的步数 Compute number of steps to execute.
// Bail if we are at the end of a feed hold and don't have a step to execute.
if (prep_segment->n_step == 0) {
if (sys.state & (STATE_HOLD|STATE_MOTION_CANCEL|STATE_SAFETY_DOOR)) {
// Less than one step to decelerate to zero speed, but already very close. AMASS
// requires full steps to execute. So, just bail.
prep.current_speed = 0.0; // NOTE: (=0.0) Used to indicate completed segment calcs for hold.
prep.dt_remainder = 0.0;
prep.steps_remaining = n_steps_remaining;
pl_block->millimeters = prep.steps_remaining/prep.step_per_mm; // Update with full steps. //zhaoxin
plan_cycle_reinitialize();
return; // Segment not generated, but current step data still retained.
}
}
// Compute segment step rate. Since steps are integers and mm distances traveled are not,
// the end of every segment can have a partial step of varying magnitudes that are not
// executed, because the stepper ISR requires whole steps due to the AMASS algorithm. To
// compensate, we track the time to execute the previous segment's partial step and simply
// apply it with the partial step distance to the current segment, so that it minutely
// adjusts the whole segment rate to keep step output exact. These rate adjustments are
// typically very small and do not adversely effect performance, but ensures that Grbl
// outputs the exact acceleration and velocity profiles as computed by the planner.
dt += prep.dt_remainder; //加上前一段未满一步所需要的执行时间 Apply previous segment partial step execute time
inv_rate = dt/(last_n_steps_remaining - steps_remaining); //计算出每步需要的时间 // Compute adjusted step rate inverse
prep.dt_remainder = (n_steps_remaining - steps_remaining)*inv_rate;//将本次循环 不足一步 所需要的执行时间// Update segment partial step time
//假设本次执行1.25步，那么dt_remainder就是0.25步所需要执行的时间
// Compute CPU cycles per step for the prepped segment.
cycles = ceil((1000000*inv_rate*60)*TICKS_PER_MICROSECOND );//每步需要的时间设置为定时器的定时时间间隔 // (cycles/step)
#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
// Compute step timing and multi-axis smoothing level.
// NOTE: AMASS overdrives the timer with each level, so only one prescalar is required.
if (cycles < AMASS_LEVEL1) { prep_segment->amass_level = 0; }
else {
if (cycles < AMASS_LEVEL2) { prep_segment->amass_level = 1; }
else if (cycles < AMASS_LEVEL3) { prep_segment->amass_level = 2; }
else { prep_segment->amass_level = 3; }
cycles >>= prep_segment->amass_level;
prep_segment->n_step <<= prep_segment->amass_level;
}
if (cycles < (1UL << 16)) { prep_segment->cycles_per_tick = cycles; } // < 65536 (4.1ms @ 16MHz)
/************************************************************
此处可以得知AMASS算法使用的是16MHz下不分频的固定定时时间
************************************************************/
else { prep_segment->cycles_per_tick = 0xffff; } // Just set the slowest speed possible.
#else
#if defined(CPU_MAP_ATMEGA328P) || defined(CPU_MAP_ATMEGA2560)
// Compute step timing and timer prescalar for normal step generation.
if (cycles < (1UL << 16)) { // < 65536 (4.1ms @ 16MHz) 4.1ms<==1(不分频)*最大可填入65535/16MHz (本分支可以产生的最长的定时时间)
prep_segment->prescaler = 1; // prescaler: 0
prep_segment->cycles_per_tick = cycles;
} else if (cycles < (1UL << 19)) { // < 524288 (32.8ms@16MHz) 32.8ms<==8分频*最大可填入65535/16MHz
prep_segment->prescaler = 2; // prescaler: 8
prep_segment->cycles_per_tick = cycles >> 3;
} else {
prep_segment->prescaler = 3; // prescaler: 64
if (cycles < (1UL << 22)) { // < 4194304 (262ms@16MHz) 262ms<<==64分频*最大可填入65535/16MHz
prep_segment->cycles_per_tick = cycles >> 6;
} else { // Just set the slowest speed possible. (Around 4 step/sec.)
prep_segment->cycles_per_tick = 0xffff;
}
}
#endif //end of CPU_MAP_ATMEGA328P & CPU_MAP_ATMEGA2560
#if defined(CPU_MAP_STM32F10X)
//将cycles值变成定时器定时的时间
//stm32的psc寄存器16位，分频可以设置的细化一些
//每一定时分支隔断条件：cycles < 65535*2^r (r=0,1,2,...)
//每一定时分支最长时间(s)=(4.5*2^r)*65535/72MHz
if (cycles < (1UL << 16)) { // < 65536 (4.1ms @ 72MHz)
prep_segment->prescaler = 8 ; // prescaler: 4.5
prep_segment->cycles_per_tick = cycles>>1;
}
else if (cycles < (1UL << 17)) { // < 131072 (8.2ms@72MHz)
prep_segment->prescaler = 8 ; // prescaler: 9
prep_segment->cycles_per_tick = cycles>>1 ;
}
else if (cycles < (1UL << 18)) { // < 262144 (16.4ms@72MHz)
prep_segment->prescaler = 17; // prescaler: 18
prep_segment->cycles_per_tick = cycles >> 2;
}
else if (cycles < (1UL << 19)) { // < 524288 (32.8ms@72MHz)
prep_segment->prescaler = 35; // prescaler: 36
prep_segment->cycles_per_tick = cycles >> 3;
}
else if (cycles < (1UL << 20)) { // < 1048576 (65.5ms@72MHz)
prep_segment->prescaler = 71; // prescaler: 72
prep_segment->cycles_per_tick = cycles >> 4;
}
else if (cycles < (1UL << 21)) { // < 2097152 (131ms@72MHz)
prep_segment->prescaler = 143; // prescaler: 144
prep_segment->cycles_per_tick = cycles >> 5;
}
else {
prep_segment->prescaler = 287; // prescaler: 288
if (cycles < (1UL << 22)) { // < 4194304 (262ms@72MHz)
prep_segment->cycles_per_tick = cycles >> 6;
} else { // Just set the slowest speed possible. (Around 4 step/sec.)
prep_segment->cycles_per_tick = 0xffff;
}
}
#endif //end of CPU_MAP_STM32F10X
#endif
// Segment complete! Increment segment buffer indices.
segment_buffer_head = segment_next_head;
if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; }
// Setup initial conditions for next segment.
if (mm_remaining > prep.mm_complete) {
// Normal operation. Block incomplete. Distance remaining in block to be executed.
pl_block->millimeters = mm_remaining; //zhaoxin
prep.steps_remaining = steps_remaining;
} else {
// End of planner block or forced-termination. No more distance to be executed.
if (mm_remaining > 0.0) { // At end of forced-termination.
// Reset prep parameters for resuming and then bail. Allow the stepper ISR to complete
// the segment queue, where realtime protocol will set new state upon receiving the
// cycle stop flag from the ISR. Prep_segment is blocked until then.
prep.current_speed = 0.0; // NOTE: (=0.0) Used to indicate completed segment calcs for hold.
prep.dt_remainder = 0.0;
prep.steps_remaining = ceil(steps_remaining);
pl_block->millimeters = prep.steps_remaining/prep.step_per_mm; // Update with full steps. //zhaoxin
plan_cycle_reinitialize();
return; // Bail!
} else { // End of planner block
// The planner block is complete. All steps are set to be executed in the segment buffer.
pl_block = NULL; // Set pointer to indicate check and load next planner block.
plan_discard_current_block();
}
}
}
}
总结
本文到此暂时告一段落，由于学习GRBL算法部分是前一段时间的事情了，然后被工作繁忙耽误了很长一段时间才抽空写了这篇博客，所以本篇博客并不完善只是初稿，后续还会继续给予本篇博客完善算法部分的详细内容，咱先发出来试试水
