Grbl limit.c代码分析

// 回零过程.

void limits_go_home(uint8_t cycle_mask)

{

if (sys.abort) { return; } // 是否系统中止.

// 初始化规划直线运动的数据寄存器，为回零运动准备，关闭主轴和冷却..

plan_line_data_t plan_data;

plan_line_data_t *pl_data = &plan_data;

memset(pl_data,0,sizeof(plan_line_data_t));

//设置规划器的条件，表明是系统运动，进给倍率无效

pl_data->condition = (PL_COND_FLAG_SYSTEM_MOTION|PL_COND_FLAG_NO_FEED_OVERRIDE);

#ifdef USE_LINE_NUMBERS //是否需要设置本指令行号

pl_data->line_number = HOMING_CYCLE_LINE_NUMBER;

#endif.

uint8_t n_cycle = (2*N_HOMING_LOCATE_CYCLE+1);//设置找零次数

uint8_t step_pin[N_AXIS];

#ifdef ENABLE_DUAL_AXIS //如果定义了双轴

uint8_t step_pin_dual;

uint8_t dual_axis_async_check;

int32_t dual_trigger_position;

#if (DUAL_AXIS_SELECT == X_AXIS)//如果X轴是双轴，计算回零运动失败的距离阈值

//如果两个限位在给定的失败距离内都触发表正常，否则失败，

float fail_distance = (-DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT/100.0)*settings.max_travel[Y_AXIS];

#else//如果Y轴是双轴，计算回零运动失败的距离阈值

float fail_distance = (-DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT/100.0)*settings.max_travel[X_AXIS];

#endif //防止回零失败距离过小或过大

fail_distance = min(fail_distance, DUAL_AXIS_HOMING_FAIL_DISTANCE_MAX);

fail_distance = max(fail_distance, DUAL_AXIS_HOMING_FAIL_DISTANCE_MIN);

int32_t dual_fail_distance = trunc(fail_distance*settings.steps_per_mm[DUAL_AXIS_SELECT]);//把距离变成步进数目

#endif

float target[N_AXIS];

float max_travel = 0.0;

uint8_t idx;

for (idx=0; idx<N_AXIS; idx++) {

// 获取对应轴步进端口的掩码

step_pin[idx] = get_step_pin_mask(idx);

#ifdef COREXY//如果是COREXY结构，设置相关掩码

if ((idx==A_MOTOR)||(idx==B_MOTOR)) { step_pin[idx] = (get_step_pin_mask(X_AXIS)|get_step_pin_mask(Y_AXIS)); }

#endif

if (bit_istrue(cycle_mask,bit(idx))) {//判断哪个轴回零，并计算回零过程可运动的最大距离

max_travel = max(max_travel,(-HOMING_AXIS_SEARCH_SCALAR)*settings.max_travel[idx]);

}

#ifdef ENABLE_DUAL_AXIS //定义双轴的步进输出端口

step_pin_dual = (1<<DUAL_STEP_BIT);

#endif

//设置进给速率

bool approach = true;

float homing_rate = settings.homing_seek_rate;

uint8_t limit_state, axislock, n_active_axis;

do {

system_convert_array_steps_to_mpos(target,sys_position);//系统位置从步进单位变成mm

axislock = 0;

#ifdef ENABLE_DUAL_AXIS

sys.homing_axis_lock_dual = 0;

dual_trigger_position = 0;

dual_axis_async_check = DUAL_AXIS_CHECK_DISABLE;

#endif

n_active_axis = 0;

for (idx=0; idx<N_AXIS; idx++) {

// 检查哪个轴回零.

if (bit_istrue(cycle_mask,bit(idx))) {

n_active_axis++;//回零轴的个数

#ifdef COREXY //如果是COREXY结构，确定A和B轴位置

if (idx == X_AXIS) {

int32_t axis_position = system_convert_corexy_to_y_axis_steps(sys_position);

sys_position[A_MOTOR] = axis_position;

sys_position[B_MOTOR] = -axis_position;

} else if (idx == Y_AXIS) {

int32_t axis_position = system_convert_corexy_to_x_axis_steps(sys_position);

sys_position[A_MOTOR] = sys_position[B_MOTOR] = axis_position;

} else {

sys_position[Z_AXIS] = 0;

}

#else

sys_position[idx] = 0;设置当前系统位置=0

#endif

// 设置回零过程的目标位置。如果回零方向是轴的负方向

. if (bit_istrue(settings.homing_dir_mask,bit(idx))) {

if (approach) { target[idx] = -max_travel; } // approach为真，目标位置为-max_travel

else { target[idx] = max_travel; }// approach为假，目标位置为max_travel

} else {// 如果回零方向是轴的正方向

if (approach) { target[idx] = max_travel; }

else { target[idx] = -max_travel; }

}

// 设置轴被锁住，即当限位触发，该轴禁止运动

axislock |= step_pin[idx];

#ifdef ENABLE_DUAL_AXIS

if (idx == DUAL_AXIS_SELECT) { sys.homing_axis_lock_dual = step_pin_dual; }

#endif

}

homing_rate *= sqrt(n_active_axis); // 总的回零速度，几个轴速度的方差

sys.homing_axis_lock = axislock; //设置回零过程中，可以被锁住的轴掩码

// 执行回零运动.

pl_data->feed_rate = homing_rate; // 设置进给速度

plan_buffer_line(target, pl_data); // 规划运动参数.

sys.step_control = STEP_CONTROL_EXECUTE_SYS_MOTION; // 设置步进控制为系统运动

st_prep_buffer(); //插补器从规划缓存区去读块数据

st_wake_up(); // 开步进定时中断

do {

if (approach) {//(approach=1表示向着限位开关方向移动

limit_state = limits_get_state(); //检查限位状态

for (idx=0; idx<N_AXIS; idx++) {

if (axislock & step_pin[idx]) { //如果本轴可以被锁定

if (limit_state & (1 << idx)) {//本轴出发限位，即被锁定

#ifdef COREXY //根据COREXY结构，放开锁定

if (idx==Z_AXIS) { axislock &= ~(step_pin[Z_AXIS]); }

else { axislock &= ~(step_pin[A_MOTOR]|step_pin[B_MOTOR]); }

#else

axislock &= ~(step_pin[idx]);

#ifdef ENABLE_DUAL_AXIS //如果是双轴，同步检查，前检查DUAL_AXIS_SELECT

if (idx == DUAL_AXIS_SELECT) { dual_axis_async_check |= DUAL_AXIS_CHECK_TRIGGER_1; } //设置DUAL_AXIS_SELECT触发。DUAL_AXIS_SELECT是X

//或Y轴中的一个

#endif

}

sys.homing_axis_lock = axislock; //重新设置轴锁定状态

#ifdef ENABLE_DUAL_AXIS //如果是双轴

if (sys.homing_axis_lock_dual) { // 如果双轴锁定为真.

if (limit_state & (1 << N_AXIS)) { //如果双轴中的镜像轴触发限位（它和Z轴共享）

sys.homing_axis_lock_dual = 0; //双轴的镜像轴锁定放开

dual_axis_async_check |= DUAL_AXIS_CHECK_TRIGGER_2; //同步检查状态为2

}

// 当双轴的同步检查触发.

if (dual_axis_async_check) {//如果检查触发（双轴的任意一个限位触发）

if (dual_axis_async_check & DUAL_AXIS_CHECK_ENABLE) {//如果检查开始

//如果同步检查已经完成两个状态，即两个限位已经触发，检查完成

if (( dual_axis_async_check & (DUAL_AXIS_CHECK_TRIGGER_1 | DUAL_AXIS_CHECK_TRIGGER_2)) == (DUAL_AXIS_CHECK_TRIGGER_1 | DUAL_AXIS_CHECK_TRIGGER_2)) {

dual_axis_async_check = DUAL_AXIS_CHECK_DISABLE;

} else {//否则当只有一个触发，计算系统位置和第一个触发时保存的位置比较

if (abs(dual_trigger_position - sys_position[DUAL_AXIS_SELECT]) > dual_fail_distance) {

//如果比较差值大于失败距离，设置回零报警，系统复位，然后循环接受实时操作指令

system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_DUAL_APPROACH);

mc_reset();

protocol_execute_realtime();

return;

}

} else { //如果双轴中的任意一个限位触发，设置检查开始，保存当前系统位置

dual_axis_async_check |= DUAL_AXIS_CHECK_ENABLE;

dual_trigger_position = sys_position[DUAL_AXIS_SELECT];

}

#endif

}

st_prep_buffer(); //插补器从规划缓存区去读块数据

// 在回零碰到限位开关过程，检查系统的实时操作指令.

if (sys_rt_exec_state & (EXEC_SAFETY_DOOR | EXEC_RESET | EXEC_CYCLE_STOP)) {

uint8_t rt_exec = sys_rt_exec_state;

// 操作指令为复位，设置回零过程复位报警状态

if (rt_exec & EXEC_RESET) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_RESET); }

// 如果门开，设置回零过程门开报警状态

if (rt_exec & EXEC_SAFETY_DOOR) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_DOOR); }

// 如果在离开限位过程中，对应轴限位依旧触发，设置回零离开报警状态

if (!approach && (limits_get_state() & cycle_mask)) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_PULLOFF); }

// 在回零前去触碰限位开关的过程中，按下停止操作，设置回零靠近失败报警状态.

if (approach && (rt_exec & EXEC_CYCLE_STOP)) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_APPROACH); }

if (sys_rt_exec_alarm) {//如果系统操作有任何报警

mc_reset(); // 复位

protocol_execute_realtime();//循环接受实时操作指令

return;

} else {

// 如果没有报警，清楚停止操作指令.

system_clear_exec_state_flag(EXEC_CYCLE_STOP);

break;

}

/*直到回零运动找到限位开关（axislock==0），当sys.homing_axis_lock某bit=0，其对应的步进输出端口没有输出。即表示，碰到限位开关后，运动停止。*/

#ifdef ENABLE_DUAL_AXIS

} while ((STEP_MASK & axislock) || (sys.homing_axis_lock_dual));

#else

} while (STEP_MASK & axislock);

#endif

st_reset(); // 插补器清零。为反方向运动准备.

delay_ms(settings.homing_debounce_delay); // 等待信号稳定

// 改变运动方向，设置移动距离

approach = !approach;

// 如果approach真，表示零点定位过程，其速度慢，允许移动距离比pulloff大；

// 如果approach假，离开零点过程，其速度快，允许移动距离为pulloff；

if (approach) {

max_travel = settings.homing_pulloff*HOMING_AXIS_LOCATE_SCALAR;

homing_rate = settings.homing_feed_rate;

} else {

max_travel = settings.homing_pulloff;

homing_rate = settings.homing_seek_rate;

}

} while (n_cycle-- > 0);//直到设置找零次数

//设置回零后的系统位置

int32_t set_axis_position;

for (idx=0; idx<N_AXIS; idx++) {

if (cycle_mask & bit(idx)) {//根据回零轴，设置回零后的位置

#ifdef HOMING_FORCE_SET_ORIGIN //如果定义回零后的位置为0

set_axis_position = 0;// 设置回零后的位置变量为0

#else//如果没有定义回零后的位置为0

if ( bit_istrue(settings.homing_dir_mask,bit(idx)) ) {//如果回零方向是向轴的正方向

set_axis_position = lround((settings.max_travel[idx]+settings.homing_pulloff)*settings.steps_per_mm[idx]);

} else {//如果回零方向是向轴的负方向

set_axis_position = lround(-settings.homing_pulloff*settings.steps_per_mm[idx]);

}

#endif

#ifdef COREXY//如果定义了COREXY结构，解耦设置A和B轴原点坐标

if (idx==X_AXIS) {

int32_t off_axis_position = system_convert_corexy_to_y_axis_steps(sys_position);

sys_position[A_MOTOR] = set_axis_position + off_axis_position;

sys_position[B_MOTOR] = set_axis_position - off_axis_position;

} else if (idx==Y_AXIS) {

int32_t off_axis_position = system_convert_corexy_to_x_axis_steps(sys_position);

sys_position[A_MOTOR] = off_axis_position + set_axis_position;

sys_position[B_MOTOR] = off_axis_position - set_axis_position;

} else {

sys_position[idx] = set_axis_position;

}

#else //否则置系统坐标

sys_position[idx] = set_axis_position;

#endif

}

sys.step_control = STEP_CONTROL_NORMAL_OP; // 系统步进控制正常标志.

}

Grbl limit.c代码分析相关推荐

ARM裸机篇---启动代码分析
ARM裸机篇---启动代码分析先搞清楚启动代码和Bootloader的区别,启动代码是指CPU复位后到进入C语言的main函数之前需要执行的那段汇编代码. 下面的代码先暂且这样吧,没啥注释的,时间关 ...
【UAV】高度控制代码分析
文章目录代码分析 MotorControl() 控制流程图代码分析 MotorControl() case PROCESS_31: {int16_t thr_temp;// 这里的定高标志在 Mo ...
Pixhawk代码分析-启动代码及入口函数
启动代码及入口函数基础知识关于坐标系 1)GeographicCoordinate System Represents position on earth with alongitude and ...
Cppcheck 1 54 C/C++静态代码分析工具
分享一下我老师大神的人工智能教程!零基础,通俗易懂!http://blog.csdn.net/jiangjunshow 也欢迎大家转载本篇文章.分享知识,造福人民,实现我们中华民族伟大复兴! Cppc ...
Python代码分析工具：PyChecker、Pylint
1 概述 PyChecker是Python代码的静态分析工具,它能够帮助查找Python代码的bug,而且能够对代码的复杂度和格式等提出警告. PyChecker可以工作在多种方式之下.首先,PyCh ...
linux内存映射起始地址,内存初始化代码分析（三）：创建系统内存地址映射
内存初始化代码分析(三):创建系统内存地址映射作者:linuxer 发布于:2016-11-24 12:08 分类:内存管理一.前言经过内存初始化代码分析(一)和内存初始化代码分析(二)的过渡, ...
对dpdk的rte_ring实现原理和代码分析
对dpdk的rte_ring实现原理和代码分析前言 dpdk的rte_ring是借鉴了linux内核的kfifo实现原理,这里统称为无锁环形缓冲队列. 环形缓冲区通常有一个读指针和一个写指针.读指针 ...
[分享]蓝屏代码查询及代码分析
系统蓝屏70%是由于内存(病毒)引起的一般来说你先系统重新装一遍要是用了一段时间还是出现这样的问题建议换根内存试试 windows蓝屏错误 1 0×00000001 不正确的函数. 2 0×0 ...
Cppcheck 1.54 C/C++静态代码分析工具
Cppcheck是一个C/C++代码分析工具,只检测那些编译器通常无法检测到的bug类型. 官方上建议让编译器提供尽量多的警告提示: 1.使用Visual C++的话,应使用警告等级4 2.使用GCC ...

Grbl limit.c代码分析

Grbl limit.c代码分析相关推荐

最新文章

热门文章