logo资料库

GRBL源代码分析.pdf

发布时间：2022-06-16 发布人：admin 分类：说明书 资料大小：4.30M 资料格式：pdf 举报 版权申诉
第1页 / 共12页
第2页 / 共12页
第3页 / 共12页
第4页 / 共12页
第5页 / 共12页
第6页 / 共12页
第7页 / 共12页
第8页 / 共12页
资料共12页，剩余部分请下载后查看
    原创地址：xufeixueren的博客的博客https://blog.csdn.net/xufeixueren/article/details/79663068 GRBL源代码分析 这段日子喜事连连，暂时把写博客的事情放下了，有时候想想好久没有写博客了，要不要写点啥呢。转念 一想，好像也没有啥值得写的心得体会，加上最近忙着结婚的事情，也就把写博客的事搁置了。周五本来 是要上班的，但是公司大厦供电系统维护，所以调休一天。借着安静的周五，加上最近一个多月研究GRBL 源代码的心得，写下这篇博客，供后来者参考学习。网上关于GRBL源代码分析的资料几乎找不到，这篇博 客里的内容大多是自己对源代码的理解，也有一部分是QQ群里的同行的心得。博客里没有写到，或者写的 不对的地方，欢迎大家留言共同探讨，共同进步。 GRBL的核心是带有梯形加减速过程的DDA直线插补算法的实现，整个GRBL源代码中包含了以下内容： （1）串口中断接收上位机的指令，包括自定义的系统命令和G代码指令； （2）串口指令解析，自定义的系统命令直接执行，G代码指令调用相关操作，这里只关注直线段、圆弧指 令的解析； （3）圆弧拆分成直线段进行插补的方法； （4）多条直线段之间转角速度优化的前瞻速度控制的方法； （5）单条线段梯形加减速过程换算成定时器定时不同时间长短来输出脉冲的方法； （6）限位条件的判断及轴自动归位的方法； （7）其它spindle、coolant接口等，这些我也不知道干嘛用的，推测是给用户二次开发预留的接口。 下面详细介绍这些模块的实现细节，串口接收、指令解析这些很容易理解，就简单说说；spindle、coolant 这些还没有琢磨透，好像用到的情况也不多，不做描述；重点还是关注圆弧拆分线段、转角速度优化、线 段梯形加减速插补和自动归位的实现方法。 一、串口接收 串口配置成中断接收和中断发送模式，并创建了串口接收环形队列和串口发送环形队列，中断接收的数 据存放在串口接收环形队列里，串口需要发送的数据放到串口发送环形队列里。当串口产生中断时，如果 接收中断标志位置位，说明接收到数据，把数据读出放到接收队列，如果发送中断标志置位，说明发送寄 存器空，把发送队列里的数据写入发送寄存器里。这就是串口处理的流程，具体的实现难度不大，就不详 细解释源码了 。 二、串口接收到的数据解析 串口接收到的数据放在串口接收环形队列里，主程序里每次从接收队列里读出一个字节，以'\r'或'\n'为标 志截取一行完整的指令，如果指令以'$'开始，说明是自定义的系统指令，其它的是G代码指令。如果是系统 自定义指令，就调用uint8_t system_execute_line(char *line)函数，里面包含了一些读取软件版本信息、读取默 认配置参数信息和把外部设置的参数信息写入eeprom里，轴归位操作等功能。如果是G代码指令，就调用 uint8_t gc_execute_line(char *line)函数，里面包含了很多G代码的指令解析过程，由于重点放在研究直线插补 算法上，没有对G代码解析源码深入分析，这里只关注里面的执行圆弧和直线插补的代码，即只需要关心 void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate)和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)两个 函数即可。 三、圆弧拆分成多线段 GRBL中把圆弧拆分成多条逼近的直线段，然后对直线段进行插补，这种方法也就是俗称的把复杂曲线拆 分成多条逼近的直线的插补方法。圆弧拆分成直线段的方法，在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)函数 里实现，下面对该函数进行详细介绍： position，圆弧起始点位置坐标，为了后面解释方便，这里设为(x0,y0,z0) target，圆弧终点坐标，这里设为(x1,y1,z1)
    offset，圆心相对于起始点的偏移向量，这里设为(rx,ry,yz)，那么圆心坐标为(x0+rx,y0+ry,z0+rz) radius，圆弧半径长度 feed_rate，轴的进给速率 invert_feed_rate，进给速率含义标志位，这里默认为零，表示进给速率的单位是min/mm，即分钟/毫米 axis_0，圆弧所在平面的第一个轴，可以是x/y/z中任意一个 axis_1，圆弧所在平面的第二个轴，可以是x/y/z中任意一个 axis_linear，除了圆弧平面之外的第三个轴，即与圆弧平面垂直的轴 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) { //圆弧所在平面的圆心坐标 float center_axis0 = position[axis_0] + offset[axis_0]; float center_axis1 = position[axis_1] + offset[axis_1]; //圆心指向圆弧起始点的向量坐标 float r_axis0 = -offset[axis_0]; float r_axis1 = -offset[axis_1]; //圆心指向圆弧终点的向量坐标 float rt_axis0 = target[axis_0] - center_axis0; float rt_axis1 = target[axis_1] - center_axis1; //计算圆心到圆弧起始点向量b(r_axis0 ,r_axis1)和圆心到圆弧终点向量c(rt_axis0 ,rt_axis1)的夹角的正切 值，注意夹角a的方向是起始点向量b逆时针转向终点向量c的角，这个在后面判断角度值符号时会用到。根 据向量夹角余弦公式，推出向量夹角正切公式，套入向量b和向量c的坐标，即可得出tana。下面公式中的 atan2是反正切函数，a=atan2(y,x)，即角度a=y/x的正切值 float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1); if (gc_state.modal.motion == MOTION_MODE_CW_ARC) { //如果圆弧顺时针移动，角度应该是负值，如果计算出的角度为正值，需要在计算出的角度基础上减 去2*pi（pi为圆周率） if (angular_travel >= 0) { angular_travel -= 2*M_PI; } } else { //如果圆弧逆时针移动，角度应该是正值，如果计算出的角度为负值，需要在计算出的角度基础上加 上2*pi（pi为圆周率） if (angular_travel <= 0) { angular_travel += 2*M_PI; } } //计算起点到终点的圆弧可以划分多少条小线段，计算方法：总共的弧长/每条小线段的长 度，angular_travel是圆弧的弧度，radius是圆弧的半径，那么它们的乘积angular_travel*radius就是圆弧的弧 长，再乘以0.5就是弧长的一半。settings.arc_tolerance是圆弧上两点之间连接的小线段到这段圆弧的最大距 离，即圆弧上的小线段到弧顶的最大距离，这里设为h，
    ，有图可知，假设线段|AB|长度的一半为k，那么有勾股定理可知，r*r=k*k+(r-h)*(r-h)。知道了r和h，那么 k*k=h*(2*r-h)。这样总共的小线段个数也就出来了，这就是下面这个公式的含义。 uint16_t segments = floor(fabs(0.5*angular_travel*radius)/ sqrt(settings.arc_tolerance*(2*radius - settings.arc_tolerance)) ); } ...... //这里是计算圆心与每条小线段所夹的角T，即上图中角AOB的余弦值和正弦值，由于角度很小，这里采 用了三角函数的泰勒级数展开公式计算cosT和sinT。cosT的二阶泰勒级数为1-T*T/2，sinT的三阶泰勒级数 为T-T*T*T/6，为了计算方便，cos_T 被放大了两倍，后面又乘上了0.5复原了。有cosT和sinT的泰勒级数公 式，cosT被放大了两倍，可以推出sinT=T*(4+cosT)/6，即下面计算sinT的公式的来源。 float cos_T = 2.0 - theta_per_segment*theta_per_segment; float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0); cos_T *= 0.5; //循环累加每一条小线段，圆的极坐标公式为x=r*cosa,y=r*sina，假设当前线段的起始坐标为(rcosa,rsina), 下一条线段比当前线段移动的角度已知为T，那么下一条线段的起始坐标为(rcos(a+T),rsin(a+T)),运算得到 rcos(a+T)=r*cosa*cosT-r*sina*sinT，rsin(a+T)=r*sina*cosT+r*cosa*sinT。由于我们知道当前线段的坐标为 (r_axis0,r_axis1)，又知道sinT和cosT的值，下一条线段的起始坐标根据公式可立即求出。 for (i = 1; i
    } 四、多线段速度规划前瞻算法 连续执行多条线段插补的时候，为了加快轴的移动速度，执行完一条直线指令后不能停下来，然后重新 启动执行下一条直线指令。而是需要保持一定的速度去执行下一条直线插补，但是由于相邻两条直线之间 有一定的夹角，导致转弯的时候，轴的速度不能过快，还要考虑两条直线执行的最大速度限制和直线头尾 速度衔接等问题，这些问题的处理方法就是前瞻算法。 GRBL中使用了环形队列的方式存储每一条直线段的信息，这个队列的名称是 block_buffer[BLOCK_BUFFER_SIZE]，这个结构体数组里存放的是线段的初速度、最大初速度限制、最大 转角速度限制、正常运行速度、加速度、线段长度信息。当G代码解析出一条线段指令或者圆弧拆分出线段 后，调用void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate)函数，把当前线段的信息存入 block_buffer队列中，然后把当前线段和队列里前一条线段结合在一起，用前瞻算法修正队列里前一条线段 的最大运行速度，以便保证在前一条线段执行结束时的速度与当前线段的初速度一致。另外，根据两条线 段的夹角确定最大转角速度，用于修正前一条线段的结束速度和当前线段的初速度不能超过最大转角速 度。下面对mc_line函数进行详细分析： target，线段移动到的最终位置，单位是mm，也就是说当前线段移动的长度是target值减去之前的所有线段 的移动长度； feed_rate，线段的最大运行速度，梯形加减速值是提前设定好保存在eeprom里的； invert_feed_rate，线段运行速度含义标志位，feed_rate有多种含义，这里我们只了解feed_rate的单位是 min/mm即可； void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate) { //如果限位使能，就检查target值是否超出了轴能到达的最远位置，如果超出了就告警限位错误，并复位系 统 if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) { limits_soft_check(target); } ...... do { //这个函数的功能很多，有很多地方都会调用它，这里调用的目的是判断有没有系统异常发生，比如系统 告警或复位，如果有异常，这个函数里处理异常的代码就会执行，没有就退出函数，继续运行 protocol_execute_runtime(); // Check for any run-time commands if (sys.abort) { return; } // Bail, if system abort. //检查block_buffer是否满，如果满就执行尝试打开线段插补执行使能开关。如果系统配置里开启了auto- cycle功能，就可以自动开始执行线段插补，block_buffer里的线段会被系统执行插补操作而空出一些空间， 这样队列就不满了，也就退出下面的do-while循环继续执行代码 if ( plan_check_full_buffer() ) { protocol_auto_cycle_start(); } // Auto-cycle start when buffer is full. else { break; } } while (1); //把当前线段的的信息添加到block_buffer队列里，这个函数里包含了前瞻算法的处理过程，在下面会详细 介绍 plan_buffer_line(target, feed_rate, invert_feed_rate); ...... } void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) { ...... for (idx=0; idx
    target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]); //target表示轴从原点移动到终点的总距离，所以当前线段的移动步数需要用target减去之前所有线段移动 的总步数 block->steps[idx] = labs(target_steps[idx]-pl.position[idx]); //获得三个轴里移动距离最远的轴移动的距离，后面DDA直线插补时会用到这个值。关于DDA插补算法的 方法后面会介绍 block->step_event_count = max(block->step_event_count, block->steps[idx]); //根据步数换算出真实移动的距离，保存在unit_vec中，后面计算两条线段夹角时会用到 delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx]; unit_vec[idx] = delta_mm; // 这个值小于零，说明这个轴需要向与原来方向相反的方向移动 if (delta_mm < 0 ) { block->direction_bits |= get_direction_pin_mask(idx); } //三个轴是正交的，知道了每个轴移动的距离，那么线段在空间里移动的真实距离是s*s=x*x+y*y+z*z，这个值在后面也 会用到 block->millimeters += delta_mm*delta_mm; } //开平方求出线段空间里移动的距离 block->millimeters = sqrt(block->millimeters); ...... for (idx=0; idxacceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value); //计算两条线段的夹角余弦值，夹角余弦公式cosa=(x1*x2+y1*y2+z1*z2)/(s1*s2)，因为两条线段是首尾相连，那么用 两条线段的向量坐标计算出来的夹角其实是它的补角，夹角和它补角的余弦值刚好取负值即可，所以下面计算夹角余弦 的方法里多了一个负号 junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx]; } } //这个用半角公式sin(a/2)=sqrt((1-cosa)/2)直接运算 float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // 计算转角最大速度v，有圆弧加速度公式可知，v*v=a*r,其中a是圆弧向心加速度，这里近似值为block- >acceleration，r是圆弧的半径。settings.junction_deviation是两条线段内切圆弧到两条线段交点的距离，这里 设为h，如下图所示，角EAD即为上面的a/2，内切圆的半径为r，那么AD的长度即为r+h，那么
    sin_theta_d2=r/(r+h)，已知h的值，那么r=h*sin_theta_d2/(1-sin_theta_d2)，那么套入v*v=a*r即可求得v*v，即 block->max_junction_speed_sqr的值。 ， 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) ); ...... //对没有优化过的线段进行优化 planner_recalculate(); } static void planner_recalculate() { ...... //如果所有的线段都已经优化过了，直接退出函数 if (block_index == block_buffer_planned) { return; } ...... //当前线段的起始速度取最大起始限制速度与末速度为零反推的最大起始速度的最小值 current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); ...... //这段代码的含义是从当前线段往前推，直到所有的线段都优化过退出循环，即每条线段的初速度不能超 过线段设置的最大初速度的限制 while (block_index != block_buffer_planned) { ......} ...... //这段代码的含义是从第一个没有优化过的线段往前，直到到达当前线段时退出循环，即每条线段的末速 度不能超过下一条线段的初速度，这样多条线段才能保持连续的速度运行 while (block_index != block_buffer_head) {} } 到这里多线段速度前瞻规划已经完成了，下面分析GRBL中怎么把带有加减速的线段转化成用定时器输出脉 冲的过程。 五、线段转化成不同频率的输出脉冲 第四节里block_buffer队列里存放的就是每条线段的详细信息，根据线段的初速度、末速度、加速度和线 段距离信息，计算出这条线段运行时轴需要移动的总步数。假定把这条线段总的运行时间截取成多个微小 的时间段DT，即可求出每个DT时间段内的平均速度，同时可以求出DT时间段内轴移动的步数n，这样就可 以求出DT内每步需要的时间dt=DT/n，把dt设定为定时器定时间隔，直到中断计数次数到达n结束。由于线 段总步数选取的是线段向量坐标(x,y,z)里的最大值，所以定时器中断里不能每次都输出脉冲，而是需要用 DDA插补算法运算出每次中断哪个轴需要输出脉冲。 这里介绍一下GRBL中用到的DDA算法的实现过程，假设线段向量坐标a(x,y,z)，选取x,y,z绝对值最大的作 为累加溢出值c=|max(x,y,z)|，假定累加初值b=c/2，那么三个轴输出脉冲的DDA算法如下：
    m=l=k=b; for(i=0;i=c) { x轴输出一个脉冲； m-=c; } if(l>=c) { y轴输出一个脉冲； l-=c; } if(k>=c) { z轴输出一个脉冲； k-=c; } } 下面开始进行源码分析，第四节里对每个线段进行预处理之后，进入主循环里protocol_auto_cycle_start()函数 和protocol_execute_runtime()函数，我们从protocol_auto_cycle_start()函数开始分析。 //当GRBL默认配置里使能了auto_start功能，就把系统执行标志sys.execute设置为EXEC_CYCLE_START，也 就是系统可以自动执行线段输出脉冲，如果没有使能auto_start，在系统运行过程中可以用串口命令手动开启 线段输出脉冲 void protocol_auto_cycle_start() { if (sys.auto_start) { bit_true_atomic(sys.execute, EXEC_CYCLE_START); } } void protocol_execute_runtime() { //省略了一些系统告警标志位的处理 ...... //当系统没有使能线段输出脉冲功能时，只调用st_prep_buffer if (rt_exec & EXEC_FEED_HOLD) { ..... st_prep_buffer(); ...... } if (rt_exec & EXEC_CYCLE_START) { ...... //修改系统状态为STATE_CYCLE，这样启动定时器后下次循环就不会在进入这里重复启动定时器了 sys.state = STATE_CYCLE; //把线段换算成定时器输出脉冲频率和脉冲个数
    st_prep_buffer(); //启动定时器开始输出脉冲 st_wake_up(); ...... } ...... //当定时器已经启动后，以后的循环就是不断的把线段换算成定时器输出脉冲 if (sys.state & (STATE_CYCLE | STATE_HOLD | STATE_HOMING)) { st_prep_buffer(); } } void st_prep_buffer() { //线段拆分成多个DT时间片，每个时间片轴运行的总步数和每步需要的时间存放在segment_buffer队里 中，while循环判断这个队列是否满，如果满了就退出循环，没满继续把线段拆分的时间片存入队列 while (segment_buffer_tail != segment_next_head) { //判断当前线段拆分时间片是否完成，如果没有完成，pl_block不为空，if里的语句不会被执行。如果 pl_block为空，说明当前线段时间片拆分完成，执行if里的语句，开始把下一条线段的信息读出来进行时间片 拆分 if (pl_block == NULL) { //从block_buffer队列中获取一条新的线段 pl_block = plan_get_current_block(); ...... //开辟新的队列st_block_buffer存放新线段拆分时间片计算过程数据 st_prep_block = &st_block_buffer[prep.st_block_index]; //记录线段每个轴的运行方向 st_prep_block->direction_bits = pl_block->direction_bits; //AMASS功能用于平滑脉冲频率太慢的线段，如果AMASS功能使能，线段步数放大 MAX_AMASS_LEVEL倍，但是定时器定时间隔将会缩短，相当于定时器中断加快了，更多次的中断累积才 输出一个脉冲，这样输出脉冲变得更平滑了 #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS]; st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS]; st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS]; st_prep_block->step_event_count = pl_block->step_event_count; #else st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS] << MAX_AMASS_LEVEL; st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS] << MAX_AMASS_LEVEL; st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS] << MAX_AMASS_LEVEL; st_prep_block->step_event_count = pl_block->step_event_count << MAX_AMASS_LEVEL; #endif //inv_2_accel 是加速度a的过程值，inv_2_accel=1/2a float inv_2_accel = 0.5/pl_block->acceleration; if (sys.state == STATE_HOLD) { //STATE_HOLD就是线段没有开始输出脉冲，这样线段的末速度为零，整个线段都是减速过程， 后面分析了线段输出脉冲的过程，再回头看这里就很简单了 ...... } else
    分享到：
    赞 收藏

    相关推荐

    资料库

    行业

    共收录7266份资料，累计15个分类，关注成员有19位，主要包括：物流,旅游,餐饮零售,网络游戏,咨询,金融,电子政务,外包,教育,制造,医疗,嵌入式,互联网,交通,电信

    热门标签

    物流 旅游 餐饮零售 网络游戏 咨询 金融 电子政务 外包 教育 制造 医疗 嵌入式 互联网 交通 电信

    最新资料