Marlin固件运动控制代码分析

实验测试

//当对每个线性移动 '标称' 值的设置进行缓冲时, 将使用此结构, 因为在源 g 代码中已指定,
//如果加速管理处于活动状态, 则可能永远无法实际达到。
typedef struct {// 由bresenham 算法用于跟踪直线的字段 long steps_x, steps_y, steps_z, steps_e;  // 每个坐标轴所需走的步数unsigned long step_event_count;           // 完成这个block所需走的步数，steps_x, steps_y, steps_z, steps_e的最大值long accelerate_until;                    // 梯形曲线中的加速距离，单位stepslong decelerate_after;                    // 加速和匀速的距离，单位steps long acceleration_rate;                   // 加速率，用来计算加速度unsigned char direction_bits;             //  这个block的方向位，“1”反向，“0”正向，每一个位代表一个轴的方向  (refers to *_DIRECTION_BIT in config.h)unsigned char active_extruder;            //  所用到的有效的挤出头#ifdef ADVANCElong advance_rate;volatile long initial_advance;volatile long final_advance;float advance;#endif// 运动规划器用于管理加速的字段float nominal_speed;                               // 额定速度，即梯形曲线的匀速阶段的速度，单位 mm/sec float entry_speed;                                 // 进入速度，即从上一个block进入到这个block的速度，单位 mm/secfloat max_entry_speed;                             // 最大进入速度，进入速度不能超过这个值，单位 mm/secfloat millimeters;                                 // 总路程，单位mm float acceleration;                                // 加速度，单位 mm/sec^2unsigned char recalculate_flag;                    // 连接处重新计算梯形速度曲线的标志unsigned char nominal_length_flag;                 // 能达到额定速度的标志// 梯形速度曲线产生器的设置参数unsigned long nominal_rate;                        //  这个block的单位为steps/sec的额定速度 unsigned long initial_rate;                        // 梯形曲线的初始速度/进入速度，单位steps/sec  unsigned long final_rate;                          // 梯形曲线的退出速度，单位steps/secunsigned long acceleration_st;                     // 单位为steps/sec^2的加速度unsigned long fan_speed;                           // 风扇速度   #ifdef BARICUDAunsigned long valve_pressure;unsigned long e_to_p_pressure;#endifvolatile char busy;                                // 正在处理这个block的标志位，“1”表示正在执行这个block
} block_t;//用于运动指令的环形缓冲器
block_t block_buffer[BLOCK_BUFFER_SIZE];            // A ring buffer for motion instructions
volatile unsigned char block_buffer_head;           // Index of the next block to be pushed
volatile unsigned char block_buffer_tail;           // Index of the block to process now

在缓冲区中添加一个新的线性移动。stepsx、y和z是mm中的绝对位置，微秒指定移动应该执行多少微秒。
为了加速计算，打电话者也必须以毫米的长度提供线路的物理长度。

*/
/* 规划直线路径 */
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
{//在存入这个字节后计算缓冲区的头指针// Calculate the buffer head after we push this byteint next_buffer_head = next_block_index(block_buffer_head);//检测block是否有空间，如果没有空间等待并执行加热管理、轴不活跃性管理、lcd更新函数，直到缓冲区有空间// If the buffer is full: good! That means we are well ahead of the robot. // Rest here until there is room in the buffer.while(block_buffer_tail == next_buffer_head){manage_heater(); manage_inactivity(); lcd_update();}#ifdef ENABLE_AUTO_BED_LEVELINGapply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif // ENABLE_AUTO_BED_LEVELING//计算各轴目标位置的step数// The target position of the tool in absolute steps// Calculate target position in absolute steps//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehowlong target[4];target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);     target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);//当有挤料的动作，且挤出头的温度低于extrude_min_temp时，不会执行挤料动作//且如果挤料的长度超过EXTRUDE_MAXLENGTH时也不会执行挤料动作#ifdef PREVENT_DANGEROUS_EXTRUDEif(target[E_AXIS]!=position[E_AXIS]){if(degHotend(active_extruder)<extrude_min_temp){position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E partSERIAL_ECHO_START;SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);}```
#ifdef PREVENT_LENGTHY_EXTRUDE
if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
{position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E partSERIAL_ECHO_START;SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
}
#endif
```}#endif//设定新的block// Prepare to set up new blockblock_t *block = &block_buffer[block_buffer_head];// Mark block as not busy (Not executed by the stepper interrupt)block->busy = false;//设定各个轴的step数// Number of steps for each axis// default non-h-bot planningblock->steps_x = labs(target[X_AXIS]-position[X_AXIS]);block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);block->steps_e *= volumetric_multiplier[active_extruder];block->steps_e *= extrudemultiply;block->steps_e /= 100;block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));//如果step_event_count低于5个step，则忽略这个指令// Bail if this is a zero-length blockif (block->step_event_count <= dropsegments){ return; }//设定风扇速度block->fan_speed = fanSpeed;#ifdef BARICUDAblock->valve_pressure = ValvePressure;block->e_to_p_pressure = EtoPPressure;#endif//设定各轴的移动方向// Compute direction bits for this block block->direction_bits = 0;
#ifndef COREXYif (target[X_AXIS] < position[X_AXIS]){block->direction_bits |= (1<<X_AXIS); }if (target[Y_AXIS] < position[Y_AXIS]){block->direction_bits |= (1<<Y_AXIS); }
#elseif ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0){block->direction_bits |= (1<<X_AXIS); }if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0){block->direction_bits |= (1<<Y_AXIS); }
#endifif (target[Z_AXIS] < position[Z_AXIS]){block->direction_bits |= (1<<Z_AXIS); }if (target[E_AXIS] < position[E_AXIS]){block->direction_bits |= (1<<E_AXIS); }//设定挤出头编号block->active_extruder = extruder;//使能各个步进电机//enable active axes#ifdef COREXYif((block->steps_x != 0) || (block->steps_y != 0)){enable_x();enable_y();}#elseif(block->steps_x != 0) enable_x();if(block->steps_y != 0) enable_y();#endif
#ifndef Z_LATE_ENABLEif(block->steps_z != 0) enable_z();
#endif// Enable allif(block->steps_e != 0){enable_e0();enable_e1();enable_e2(); }/* 设定进给速率 */if (block->steps_e == 0){if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;}else{if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;} /* 计算各个步进电机要移动的步数 */float delta_mm[4];#ifndef COREXYdelta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];#elsedelta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS];delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS];#endifdelta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*volumetric_multiplier[active_extruder]*extrudemultiply/100.0;//计算移动的距离if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments ){block->millimeters = fabs(delta_mm[E_AXIS]);} else{block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));}float inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple divides ```
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
```float inverse_second = feed_rate * inverse_millimeters;//计算排入行程的block数int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
#ifdef OLD_SLOWDOWNif(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1)feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
#endif#ifdef SLOWDOWN//  segment time im micro secondsunsigned long segment_time = lround(1000000.0/inverse_second);if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5))){if (segment_time < minsegmenttime){ // buffer is draining, add extra time.  The amount of time added increases if the buffer is still emptied more.inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));#ifdef XY_FREQUENCY_LIMITsegment_time = lround(1000000.0/inverse_second);#endif}}
#endif//  END OF SLOW DOWN SECTION    //计算block的额定速度（mm/sec）和额定速率（step/sec）block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0// Calculate and limit speed in mm/sec for each axisfloat current_speed[4];float speed_factor = 1.0; //factor <=1 do decrease speedfor(int i=0; i < 4; i++){current_speed[i] = delta_mm[i] * inverse_second;if(fabs(current_speed[i]) > max_feedrate[i])speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));}// Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)// Check and limit the xy direction change frequencyunsigned char direction_change = block->direction_bits ^ old_direction_bits;old_direction_bits = block->direction_bits;segment_time = lround((float)segment_time / speed_factor);if((direction_change & (1<<X_AXIS)) == 0){x_segment_time[0] += segment_time;}else{x_segment_time[2] = x_segment_time[1];x_segment_time[1] = x_segment_time[0];x_segment_time[0] = segment_time;}if((direction_change & (1<<Y_AXIS)) == 0){y_segment_time[0] += segment_time;}else{y_segment_time[2] = y_segment_time[1];y_segment_time[1] = y_segment_time[0];y_segment_time[0] = segment_time;}long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);if(min_xy_segment_time < MAX_FREQ_TIME)speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
#endif// Correct the speed  if( speed_factor < 1.0){for(unsigned char i=0; i < 4; i++){current_speed[i] *= speed_factor;}block->nominal_speed *= speed_factor;block->nominal_rate *= speed_factor;}//为梯形速度控制算法计算加速度并限幅// Compute and limit the acceleration rate for the trapezoid generator.  float steps_per_mm = block->step_event_count/block->millimeters;if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0){block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2}else{block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2//为每个轴的加速度进行限幅// Limit acceleration per axisif(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];}block->acceleration = block->acceleration_st / steps_per_mm;//block->acceleration_rate是为了控制器计算速度（step/s）而设定的加速度值，(F_CPU / 8.0)为定时器的时钟频率，(16777216.0 = <<24)//加速度阶段的速度正常算法为：acc_step_rate = accelerate_time/(F_CPU / 8.0) * block->acceleration_st//acc_step_rate和block->acceleration_st都为整数，利用下面设定该参数的方法可以避免产生小数(通过移位来实现的)//加速度阶段的速度的实际计算方法：acc_step_rate = accelerate_time * (block->acceleration_st * (16777216.0 / (F_CPU / 8.0))>>24block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0))); //为了不产生小数//预设不会做连接速度的计算
#if 0  // Use old jerk for now// Compute path unit vectordouble unit_vec[3];unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.// Let a circle be tangent to both previous and current path line segments, where the junction// deviation is defined as the distance from the junction to the closest edge of the circle,// colinear with the circle center. The circular segment joining the two paths represents the// path of centripetal acceleration. Solve for max velocity based on max acceleration about the// radius of the circle, defined indirectly by junction deviation. This may be also viewed as// path width or max_jerk in the previous grbl version. This approach does not actually deviate// from path, but used as a robust way to compute cornering speeds, as it takes into account the// nonlinearities of both the junction angle and junction velocity.double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {// 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.double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;```
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {vmax_junction = min(previous_nominal_speed,block->nominal_speed);// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.if (cos_theta > -0.95) {// Compute maximum junction velocity based on maximum acceleration and junction deviationdouble sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.vmax_junction = min(vmax_junction,sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );}
}
```}
#endif//设定连接处的最大速度// Start with a safe speedfloat vmax_junction = max_xy_jerk/2; float vmax_junction_factor = 1.0; if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2) vmax_junction = min(vmax_junction, max_z_jerk/2);if(fabs(current_speed[E_AXIS]) > max_e_jerk/2) vmax_junction = min(vmax_junction, max_e_jerk/2);vmax_junction = min(vmax_junction, block->nominal_speed);float safe_speed = vmax_junction;if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));//    if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {vmax_junction = block->nominal_speed;//    }//计算vmax_junction_factorif (jerk > max_xy_jerk) {vmax_junction_factor = (max_xy_jerk/jerk);} if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {vmax_junction_factor= min(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));} if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) {vmax_junction_factor = min(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));} //计算vmax_junctionvmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed}//计算max_entry_speedblock->max_entry_speed = vmax_junction;// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);block->entry_speed = min(vmax_junction, v_allowable);// Initialize planner efficiency flags// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then// the current block and next block junction speeds are guaranteed to always be at their maximum// junction speeds in deceleration and acceleration, respectively. This is due to how the current// block nominal speed limits both the current and next maximum junction speeds. Hence, in both// the reverse and forward planners, the corresponding block junction speed will always be at the// the maximum junction speed and may always be ignored for any speed reduction checks.if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }else { block->nominal_length_flag = false; }block->recalculate_flag = true; // Always calculate trapezoid for new block// Update previous path unit_vector and nominal speedmemcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]previous_nominal_speed = block->nominal_speed;#ifdef ADVANCE// Calculate advance rateif((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {block->advance_rate = 0;block->advance = 0;}else {long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * (current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUTION_AREA * EXTRUTION_AREA)*256;block->advance = advance;if(acc_dist == 0) {block->advance_rate = 0;} else {block->advance_rate = advance / (float)acc_dist;}}/*SERIAL_ECHO_START;SERIAL_ECHOPGM("advance :");SERIAL_ECHO(block->advance/256.0);SERIAL_ECHOPGM("advance rate :");SERIAL_ECHOLN(block->advance_rate/256.0);*/
#endif // ADVANCE//计算梯形加减速（*重点*）calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed,safe_speed/block->nominal_speed);// Move buffer headblock_buffer_head = next_buffer_head;//记录目前的位置// Update positionmemcpy(position, target, sizeof(target)); // position[] = target[]//重新对所有的block做路径规划（*重点*）planner_recalculate();//使能步进电机中断st_wake_up();
}

Marlin固件运动控制代码分析相关推荐

机械臂——arduino、marlin固件、printrun软件
最近了解到,在市面上大多数机械臂控制都采用的arduino这个开源硬件来控制的,而我发现既然会单片机,就没有必要采用arduino来控制了,arduino只是一种为了简化编程而开发一种软硬件控制平台, ...
【Android SDM660源码分析】- 02 - UEFI XBL QcomChargerApp充电流程代码分析
[Android SDM660源码分析]- 02 - UEFI XBL QcomChargerApp充电流程代码分析一.加载 UEFI 默认应用程序 1.1 LaunchDefaultBDSApps ...
Marlin固件之—：基础入门与测试
一.Marlin的简单介绍 Marlin固件是一个3D打印的开源固件,3D打印固件有许多,Marlin最为健全和强大,当然相对也会复杂一些.使用Gcode控制爱,Gcode是数控机床等工控控制使用范围 ...
Marlin固件之二：源代码详解与移植
由于需要进行固件定制化,Marlin固件太过于强大和紧凑,我对这个固件进行了裁剪,只剩下主枝干,实现功能的定制和裁剪.以下的代码详解是基于我已经移植在stm32上面的一个程序进行的.
[分享]蓝屏代码查询及代码分析
系统蓝屏70%是由于内存(病毒)引起的一般来说你先系统重新装一遍要是用了一段时间还是出现这样的问题建议换根内存试试 windows蓝屏错误 1 0×00000001 不正确的函数. 2 0×0 ...
静态代码分析工具列表分析---代码分析工具列表（30款工具）
本文是一个静态代码分析工具的清单,共有30个工具.包括4个.NET工具.2个Ada工具.7个C++工具.4个Java工具.2个JavaScript工具.1个Opa工具.2个Packaging工具.3个 ...
marlin固件烧录教程_Marlin固件配置教程详解
首先从Marlin固件GitHub下载固件源代码,也可从Makeboard网盘下载. Marlin固件主要分为两个版本,一个是1.0.2-2版本,是稳定版,已经一年没怎么更新了,功能比较少,网上的教程 ...
3D打印机Marlin固件双Z轴设置
3D打印机Marlin固件双Z轴设置在3D打印机Marlin固件的最新版本2.1.1中,设置双Z轴和老版本有一些改动.记录一下如何在最新版本的Marlin固件中设置双Z轴. 以MKS GEN_L V ...
飞腾CPU虚拟化相关代码分析（一）
飞腾CPU虚拟化相关代码分析(一)-- 函数el2_setup 函数el2_setup是ARM64体系结构下Linux内核运行的第一个和虚拟化相关的函数. 相关概念 ARM64支持两种虚拟方式:Hyp ...
Firmware固件提取及分析技术
概念固件(firmware)是一种写入硬件设备的软件,作用是对应用和各项系统功能实时控制.固件中包含底层代码,这些代码能实现软件对硬件的操作.运行固件的设备称为嵌入式系统,嵌入式系统的硬件资源在存储 ...

Marlin固件运动控制代码分析

实验测试

Marlin固件运动控制代码分析相关推荐

最新文章

热门文章