开源cnc项⽬Marlin2.0运动控制部分代码理解
本⽂主要梳理Marlin2.0⼯程代码中关于运动控制部分的理解。Marlin1.0⼯程代码⽤C语⾔写的，阅读起来⽐较容易。Marlin1.0主要核⼼算法包括圆弧插补、速度前瞻、转⾓速度圆滑、梯形速度规划、Brenham多轴插补。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 &offt, // Center of rotation relative to current_position
//相对于当前位current_position的圆⼼位置，有center_position=current_position+offt
const uint8_t clockwi // Clockwi? //顺时针还是逆时针插补
)
先列出圆弧插补原理⽰意图：
圆⼼坐标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 = -offt; //Ps为当前点，O点坐标为(Ps.x+offt.x,Ps.y+offt.y)，则向量OPs=（-offt.x,-offt.y）=-offt。
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+offt.x,center_Q=ps.y+offt.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*OP if (angular_travel < 0) angular_travel += RADIANS(360);
#ifdef MIN_ARC_SEGMENTS
uint16_t min_gments = CEIL((MIN_ARC_SEGMENTS) * (angular_travel / RADIANS(360)));
NOLESS(min_gments, 1U);
#el
constexpr uint16_t min_gments = 1;
#endif
if (clockwi) 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_gments = 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 gments = FLOOR(mm_of_travel / (MM_PER_ARC_SEGMENT));
NOLESS(gments, min_gments);
将N个⼩圆弧当成直线进⾏插补：
for (uint16_t i = 1; i < gments; i++) { // Iterate (gments-1) times
星火英语网站........省略代码
const float cos_Ti = cos(i * theta_per_gment),
sin_Ti = sin(i * theta_per_gment);
//计算OPi，OPi=(rcos(θs+θi),rsin(θs+θi)),θi=i*theta_per_gment
rvec.a = -offt[0] * cos_Ti + offt[1] * sin_Ti;
rvec.b = -offt[0] * sin_Ti - offt[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_gment);
#el
raw[l_axis] += linear_per_gment;
#endif
raw.e += extruder_per_gment;
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_gment函数，
bool Planner::buffer_gment(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 fal;
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 ud by the motion planner to manage acceleration
float nominal_speed_sqr, // The nominal speed for this block in (mm/c)^2
entry_speed_sqr, // Entry speed at previous-current junction in (mm/c)^2
max_entry_speed_sqr, // Maximum allowable junction entry speed in (mm/c)^2
millimeters, // The total travel of this block in mm
acceleration; // acceleration mm/c^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)orientated
#el
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 crui_rate, // The actual crui rate to u, between end of the acceleration pha and start of deceleration pha
acceleration_time, // Acceleration time and deceleration time in STEP timer counts
sheezus
deceleration_time,
acceleration_time_inver, // Inver of acceleration and deceleration periods, expresd as integer. Scale depends on CPU being ud deceleration_time_inver;
#el
uint32_t acceleration_rate; // The acceleration rate ud for acceleration calculation
#endif
uint8_t direction_bits; // The direction bit t for this block (refers to *_DIRECTION_BIT in config.h)
// Advance extrusion
#if ENABLED(LIN_ADVANCE)
bool u_advance_lead;
uint16_t advance_speed, // STEP timer value for extruder speed offt 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/c
initial_rate, // The jerk-adjusted step rate at start of block
final_rate, // The minimal rate at exit
acceleration_steps_per_s2; // acceleration steps/c^2
behavior#if HAS_CUTTER
#if HAS_CUTTER
cutter_power_t cutter_power; // Power level for Spindle, Lar, 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 gment_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
proof是什么意思*
* Returns true is movement is acceptable, fal otherwi
*/
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
#endifno matter
#if IS_KINEMATIC && DISABLED(CLASSIC_JERK)
, const xyze_float_t &delta_mm_cart
#endif
kailas, 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;
#el
constexpr int32_t de = 0;
#endif
uint8_t dm = 0;
#if CORE_IS_XY
......⼀⼤堆宏，看着好累
#el
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
