提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档
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 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 // 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 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//①
//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 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 millimeters; }//暂无
if (feed_rate millimeters; // 取第三边的倒数Inverse millimeters to remove multiple float divides
junction_cos_theta = 0;
for (idx=0; 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 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 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 entry_speed_sqr)
{
next->entry_speed_sqr = entry_speed_sqr; // Always 
