开源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*OP  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
#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

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。