12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931193219331934193519361937193819391940194119421943194419451946194719481949195019511952195319541955195619571958195919601961196219631964196519661967196819691970197119721973197419751976197719781979198019811982198319841985198619871988198919901991199219931994199519961997199819992000200120022003200420052006200720082009201020112012201320142015201620172018201920202021202220232024202520262027202820292030203120322033203420352036203720382039204020412042204320442045204620472048204920502051205220532054205520562057205820592060206120622063206420652066206720682069207020712072207320742075207620772078207920802081208220832084208520862087208820892090209120922093209420952096209720982099210021012102210321042105210621072108210921102111211221132114211521162117211821192120212121222123212421252126212721282129213021312132213321342135213621372138213921402141214221432144214521462147214821492150215121522153215421552156215721582159216021612162216321642165216621672168216921702171217221732174217521762177217821792180218121822183218421852186218721882189219021912192219321942195219621972198219922002201220222032204220522062207220822092210221122122213221422152216221722182219222022212222222322242225222622272228222922302231223222332234223522362237223822392240224122422243224422452246224722482249225022512252225322542255225622572258225922602261226222632264226522662267226822692270227122722273227422752276227722782279228022812282228322842285228622872288228922902291229222932294229522962297229822992300230123022303230423052306230723082309231023112312231323142315231623172318231923202321232223232324232523262327232823292330233123322333233423352336233723382339234023412342234323442345234623472348234923502351235223532354235523562357235823592360236123622363236423652366236723682369237023712372237323742375237623772378237923802381238223832384238523862387238823892390239123922393239423952396239723982399240024012402240324042405240624072408240924102411241224132414241524162417241824192420242124222423242424252426242724282429243024312432243324342435243624372438243924402441244224432444244524462447244824492450245124522453245424552456245724582459246024612462246324642465246624672468246924702471247224732474247524762477247824792480248124822483248424852486248724882489249024912492249324942495249624972498249925002501250225032504250525062507250825092510251125122513251425152516251725182519252025212522252325242525 |
- //============================================================================//
- //2020.05.14 00:18 LXZ
- // 1、修改了部分定义
- // 2、将以往获取物理轴表大小的部分全替换成插补任务对象的变量
- // 3、计算加减速表大小参数修改保存至插补任务对象中
- //2020.05.19 17:19 LXZ
- // 1、修改进给输出为比例运算后输出
- // 2、修改设置轴接口为interp_set_axis,并增加比例参数
- //2020.05.19 17:19 LXZ
- // 1、修正进给比例的速度没对应修正问题
- //2020.05.26 16:34 LXZ
- // 1、去除没有测试的多余的函数接口
- // 2、增加了插补路径接口,更新相关接口的参数,主要是增停止减速
- // 3、更新了插补运算算法,现在减速度是额外设置的,
- // 4、整天结构调整应付连续插补功能,运行代码重写
- // 5、重新实现停止方法,去除所有停止相关代码,现在是设置停止位进行进给停止输出,
- // 真正的停止由外部自己处理,因为连续插补结构无法实现插补减速
- //2020.08.07 11:15
- // 1、移植比例放大功能
- // 2、增加放大进给的代码。
- //2021.08.21 12:55
- // 1、增加pi_round比例放大函数,带了四舍五入
- // 2、改进circle_get_eigth_of_part函数,利用当前进给实现精确的相位换算,能解决椭圆变速点定位困难的问题
- // 3、改进插补函数interp_begin_arc_3pt_task、interp_begin_arc_task、interp_begin_line_task等接口,增加对坐标、进给的
- // 比例变换。增加了interp_begin_ellipse_task函数用于启动椭圆路径加工。
- // 4、改进interp_work_arc_task、interp_work_line_task、interp_stop_arc_task、interp_stop_line_task等插补算法,通过算
- // 法的改进优化实现了进给的粒子细化,以前进给为齿轮比的倒数的倍数,粒子化后为单位脉冲,能实现更平稳的停止启动以及
- // 加速段的平滑度
- // 5、将共享加速表独立化到虚拟轴对象中,改进加减速算法为伪S曲线(中间断为直线),实现更加合理的加减速,合理避免了速度
- // 曲线的突变,更快加速到目标速度,但是会增加额外的内存,目前默认大小为1K
- // 6、圆弧算法被替换成了椭圆算法,从而兼容圆弧与椭圆的路径加工
- // 7、后级进给系统从当前的齿轮比倒数倍数还原成了以前的了1:1,并取消了下一级进给缓冲,因为现在后级倍率已经被取消,这
- // 部分不需要了
- //2021.08.25 00:12
- // 1、增加了椭圆开方结果做四舍五入,可以减少X\Y圆弧查补因为齿轮比差异太大产生一些误判,
- // 2、稍微平滑了加速度表
- // 3、取消了连续插补路径的位置补偿,因为现在不再是1:1的比列了,引入齿轮比后,路径会有比例变化,这时候采用这种方式去
- // 计算位置修正,本身也是会带来误差的,还不如不做修正
- //2022.04.11 10:47 LXZ
- // 通过优化8分圆判断算法,达到让椭圆弧加工过程不会速度剧烈波动造成机架抖动
- // 1、增加ellipse_get_eight_of_part_by_xaxis与ellipse_get_eight_of_part_by_yaxis两个判断8分圆位置的函数
- // X轴为长轴时用ellipse_get_eight_of_part_by_xaxis,Y轴为长轴时用ellipse_get_eight_of_part_by_yaxis
- // 删除以前判断圆8分圆位置的函数,该函数不再使用,以前使用的地方用上面两个函数代替
- // 2、增加interp_calc_arc_refrence,该函数是内部调用的,用于计算圆弧的8分圆参考位置(绝对值)
- // 3、修改加速过程为直线加减速
- //============================================================================//
- #include "axis_interp.h"
- #include <math.h>
- #include <stdio.h>
- #include <stdlib.h>
- #include <string.h>
- #define M_PI 3.14159265358979323846
- /**
- * 对数值进行比例放大后执行四舍五入
- *
- * @author LXZ (032620)
- *
- * @param interp 插补对象
- * @param start_point 开始坐标
- * @param end_point 终点坐标
- * @param speed 速度
- */
- static int pi_round(float value, float ratio) {
- if (value > 0) {
- return (int)((value * ratio) + 0.5);
- } else {
- return (int)((value * ratio) - 0.5);
- }
- }
- /**
- * 初始化插补对象的参数值
- *
- * @author LXZ (033120)
- *
- * @param interp 插补对象
- * @param ref_clcok 参考时钟
- */
- void interp_init(interp_task_t *interp, int ref_clcok) {
- int i = 0;
- memset(interp, 0, sizeof(interp_task_t));
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- interp->axis[i].id = i;
- }
- interp->ref_clock = ref_clcok * INTERP_PERIOD;
- }
- /**
- * 关闭插补轴,关闭后轴将不参与计算与插补运动,如果是X轴或者Y轴,圆弧插补会强制打开
- *
- * @author LXZ (032620)
- *
- * @param interp 插补对象
- * @param axis 轴编号
- */
- void interp_axis_enable(interp_task_t *interp, char axis_id) {
- interp->axis[axis_id].flg = 1;
- }
- /**
- * 关闭插补轴,关闭后轴将不参与计算与插补运动,如果是X轴或者Y轴,圆弧插补会强制打开
- *
- * @author LXZ (032620)
- *
- * @param interp 插补对象
- * @param axis 轴编号
- */
- void interp_axis_disable(interp_task_t *interp, char axis_id) {
- interp->axis[axis_id].flg = 0;
- }
- /**
- * 为指定插补轴设置物理轴对象
- *
- * @author LXZ (032620)
- *
- * @param interp 插补对象
- * @param axis_id 轴编号
- * @param axis 轴对象
- * @param ratio 输出比例
- */
- void interp_set_axis(interp_task_t *interp, char axis_id, void *axis, float ratio, float max_acc) {
- interp->axis[axis_id].axis = axis;
- interp->axis[axis_id].ratio = ratio;
- interp->axis[axis_id].max_acc = max_acc;
- if (interp->axis[axis_id].max_acc > interp->axis[axis_id].ratio) {
- interp->axis[axis_id].max_acc = interp->axis[axis_id].ratio;
- }
- }
- /**
- * 对插补参数预初始化
- *
- * @author LXZ (032620)
- *
- * @param interp 插补对象
- */
- static void interp_task_init(interp_task_t *interp, int speed, int stop_speed) {
- int i = 0;
- float dst_speed = 0;
- int max_speed;
- int speed_table_size = 0;
- int axis_stop_speed = 0;
- //基本参数赋值
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- //interp->axis[i].last_period = PERIOD_MAX_VALUE;
- //interp->axis[i].cur_speed = 0;
- interp->axis[i].dec_speed = 0;
- interp->axis[i].dec_position = 0;
- interp->axis[i].cur_delta = interp->axis[i].total_delta;
- if (interp->axis[i].flg) {
- interp_axis_calc_speed(&(interp->axis[i]), speed);
- }
- interp->axis[i].last_feed = 0;
- //关键参考坐标赋值
- interp->axis[i].cur_posi = interp->start_point.vector[i];
- interp->axis[i].target_posi = interp->end_point.vector[i];
- interp->axis[i].real_posi = interp->start_point.vector[i];
- interp->axis[i].start_posi = interp->start_point.vector[i];
- //轴真实位置初始值
- dst_speed = 0;
- max_speed = (int)(speed * interp->axis[i].ratio) / 1000;
- axis_stop_speed = (int)(stop_speed * interp->axis[i].ratio) / 1000;
- if (max_speed == 0) max_speed = 1;
- speed_table_size = 0;
- interp->axis[i].stop_speed = 0;
- while (dst_speed < max_speed) {
- dst_speed += 1;
- interp->axis[i].speed_table[speed_table_size] = (int)dst_speed;
- if (dst_speed <= axis_stop_speed) {
- interp->axis[i].stop_speed = speed_table_size;
- }
- speed_table_size++;
- }
- interp->axis[i].max_speed = speed_table_size;
- }
- interp->last_quadrant = 0;
- }
- /**
- * 复位插补任务,包括路径参数为0.开始位置为0
- *
- * @author LXZ (032620)
- *
- * @param interp 插补对象
- */
- void interp_task_reset(interp_task_t *interp) {
- int i = 0;
- //缓冲清0
- interp->cur_path_index = 0;
- interp->path_count = 0;
- //当前虚拟轴状态复位
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- interp->axis[i].cur_posi = 0;
- interp->axis[i].cur_speed = 0;
- interp->axis[i].real_posi = 0;
- interp->axis[i].axis_position = 0;
- interp->axis[i].cur_delta = 0;
- interp->axis[i].last_period = PERIOD_MAX_VALUE;
- interp->axis[i].dec_speed = 0;
- interp->axis[i].dec_position = 0;
- interp->axis[i].last_feed = 0;
- }
- interp->last_quadrant = 0;
- memset(&interp->paths[0], 0, sizeof(interp->paths[0]));
- //工作缓冲状态复位
- interp->cur_buffer_index = 0;
- interp->feed_count = 0;
- for (i = 0; i < INTERP_WORK_BUFFER_NUMBER; i++) {
- interp->work_buffer[i].count = 0;
- }
- interp->stop_flg = 0; //关闭停止位,这时候周期进给任务会使能
- }
- /**
- * 算当前坐标在圆弧中所处的象限
- *
- * @author LXZ (032620)
- *
- * @param sn 圆弧方向
- * @param x 相对圆心的X轴坐标值
- * @param y 相对圆心的Y轴坐标值
- *
- * @return int 象限1~4
- */
- static int arc_get_quadrant(int sn, int x, int y) {
- int quadrant = 0;
- if ((x > 0) && (y > 0)) quadrant = 1;
- else if ((x < 0) && (y > 0)) quadrant = 2;
- else if ((x < 0) && (y < 0)) quadrant = 3;
- else if ((x > 0) && (y < 0)) quadrant = 4;
- // 如果在坐标轴上, 按顺序 判定
- if (sn == 0) { // 逆圆
- if ((x > 0) && (y == 0)) quadrant = 1;
- else if ((x == 0) && (y > 0)) quadrant = 2;
- else if ((x < 0) && (y == 0)) quadrant = 3;
- else if ((x == 0) && (y < 0)) quadrant = 4;
- } else { // 1 顺圆
- if ((x == 0) && (y > 0)) quadrant = 1;
- else if ((x < 0) && (y == 0)) quadrant = 2;
- else if ((x == 0) && (y < 0)) quadrant = 3;
- else if ((x > 0) && (y == 0)) quadrant = 4;
- }
- // 第几象限
- return quadrant;
- }
- /**
- * 为了不经过角度计算而开发的识别椭圆8分圆区域扩展(X轴)
- *
- * @author LXZ (040922)
- *
- * @param sn 方向
- * @param x 相对于圆心的X轴坐标
- * @param y 相对于圆心的Y轴坐标
- * @param ratio x,y轴比列
- * @param last_quadrant 上一次的象限,防止象限来回跳
- *
- * @return int
- * 1~8分别代码8分圆8个象限,跟四象限一样,每个象限再细分两半
- */
- static int ellipse_get_eight_of_part_by_xaxis(int sn, int x, int y, int refrence, int last)
- {
- int quadrant = 0;
- if ((x > 0) && (y > 0)) quadrant = 1;
- else if ((x < 0) && (y > 0)) quadrant = 2;
- else if ((x < 0) && (y < 0)) quadrant = 3;
- else if ((x > 0) && (y < 0)) quadrant = 4;
- else if (x == 0)
- {
- if (sn == 0) // 逆圆
- {
- if (y > 0)quadrant = 2;
- else quadrant = 4;
- }
- else //顺圆
- {
- if (y > 0) quadrant = 1;
- else quadrant = 3;
- }
- }
- else if (y == 0)
- {
- if (sn == 0) //逆圆
- {
- if (x > 0) quadrant = 1;
- else quadrant = 3;
- }
- else //顺圆4
- {
- if (x > 0) quadrant = 4;
- else quadrant = 2;
- }
- }
- if (x < 0) x = -x;
- //求得8分圆像限位置
- switch (quadrant)
- {
- case 1:
- if (sn == 0) //逆圆
- {
- if (x >= refrence)
- {
- //第二象限在逆圆情况下不可能2转向1
- if (last != 2)
- quadrant = 1;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在逆圆情况下不可能3转向2
- if (last != 3)
- quadrant = 2;
- else
- quadrant = last;
- }
- }
- else //顺圆
- {
- if (x <= refrence)
- {
- //第二象限在顺圆情况下不可能1转向2
- if (last != 1)
- quadrant = 2;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在顺圆情况下不可能2转向3
- if (last != 2)
- quadrant = 3;
- else
- quadrant = last;
- }
- }
- break;
- case 2:
- if (sn == 0) //逆圆
- {
- if (x >= refrence)
- {
- //第二象限在逆圆情况下不可能2转向1
- if (last != 5)
- quadrant = 4;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在逆圆情况下不可能3转向2
- if (last != 4)
- quadrant = 3;
- else
- quadrant = last;
- }
- }
- else //顺圆
- {
- if (x <= refrence)
- {
- //第二象限在顺圆情况下不可能1转向2
- if (last != 2)
- quadrant = 3;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在顺圆情况下不可能2转向3
- if (last != 3)
- quadrant = 4;
- else
- quadrant = last;
- }
- }
- break;
- case 3:
- if (sn == 0) //逆圆
- {
- if (x >= refrence)
- {
- //第二象限在逆圆情况下不可能2转向1
- if (last != 6)
- quadrant = 5;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在逆圆情况下不可能3转向2
- if (last != 7)
- quadrant = 6;
- else
- quadrant = last;
- }
- }
- else //顺圆
- {
- if (x <= refrence)
- {
- //第二象限在顺圆情况下不可能1转向2
- if (last != 5)
- quadrant = 6;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在顺圆情况下不可能2转向3
- if (last != 4)
- quadrant = 5;
- else
- quadrant = last;
- }
- }
- break;
- case 4:
- if (sn == 0) //逆圆
- {
- if (x >= refrence)
- {
- //第二象限在逆圆情况下不可能2转向1
- if (last != 1)
- quadrant = 8;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在逆圆情况下不可能3转向2
- if (last != 8)
- quadrant = 7;
- else
- quadrant = last;
- }
- }
- else //顺圆
- {
- if (x <= refrence)
- {
- //第二象限在顺圆情况下不可能1转向2
- if (last != 6)
- quadrant = 7;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在顺圆情况下不可能2转向3
- if (last != 7)
- quadrant = 8;
- else
- quadrant = last;
- }
- }
- break;
- }
- return quadrant;
- }
- /**
- * 为了不经过角度计算而开发的识别椭圆8分圆区域扩展(Y轴)
- *
- * @author LXZ (040922)
- *
- * @param sn 方向
- * @param x 相对于圆心的X轴坐标
- * @param y 相对于圆心的Y轴坐标
- * @param ratio x,y轴比列
- * @param last_quadrant 上一次的象限,防止象限来回跳
- *
- * @return int
- * 1~8分别代码8分圆8个象限,跟四象限一样,每个象限再细分两半
- */
- static int ellipse_get_eight_of_part_by_yaxis(int sn, int x, int y,
- int refrence,int last)
- {
- int quadrant = 0;
- if ((x > 0) && (y > 0)) quadrant = 1;
- else if ((x < 0) && (y > 0)) quadrant = 2;
- else if ((x < 0) && (y < 0)) quadrant = 3;
- else if ((x > 0) && (y < 0)) quadrant = 4;
- else if (x == 0)
- {
- if (sn == 0) // 逆圆
- {
- if (y > 0)quadrant = 2;
- else quadrant = 4;
- }
- else //顺圆
- {
- if (y > 0) quadrant = 1;
- else quadrant = 3;
- }
- }
- else if (y == 0)
- {
- if (sn == 0) //逆圆
- {
- if (x > 0) quadrant = 1;
- else quadrant = 3;
- }
- else //顺圆4
- {
- if (x > 0) quadrant = 4;
- else quadrant = 2;
- }
- }
- if (y < 0) y = -y;
- //求得8分圆像限位置
- switch (quadrant)
- {
- case 1:
- if (sn == 0) //逆圆
- {
- if (y >= refrence)
- {
- //第二象限在逆圆情况下不可能2转向1
- if (last != 3)
- quadrant = 2;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在逆圆情况下不可能3转向2
- if (last != 2)
- quadrant = 1;
- else
- quadrant = last;
- }
- }
- else //顺圆
- {
- if (y <= refrence)
- {
- //第二象限在顺圆情况下不可能1转向2
- if (last != 8)
- quadrant = 1;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在顺圆情况下不可能2转向3
- if (last != 1)
- quadrant = 2;
- else
- quadrant = last;
- }
- }
- break;
- case 2:
- if (sn == 0) //逆圆
- {
- if (y >= refrence)
- {
- //第二象限在逆圆情况下不可能2转向1
- if (last != 4)
- quadrant = 3;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在逆圆情况下不可能3转向2
- if (last != 5)
- quadrant = 4;
- else
- quadrant = last;
- }
- }
- else //顺圆
- {
- if (y <= refrence)
- {
- //第二象限在顺圆情况下不可能1转向2
- if (last != 3)
- quadrant = 4;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在顺圆情况下不可能2转向3
- if (last != 2)
- quadrant = 3;
- else
- quadrant = last;
- }
- }
- break;
- case 3:
- if (sn == 0) //逆圆
- {
- if (y >= refrence)
- {
- //第二象限在逆圆情况下不可能2转向1
- if (last != 7)
- quadrant = 6;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在逆圆情况下不可能3转向2
- if (last != 6)
- quadrant = 5;
- else
- quadrant = last;
- }
- }
- else //顺圆
- {
- if (y <= refrence)
- {
- //第二象限在顺圆情况下不可能1转向2
- if (last != 4)
- quadrant = 5;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在顺圆情况下不可能2转向3
- if (last != 5)
- quadrant = 6;
- else
- quadrant = last;
- }
- }
- break;
- case 4:
- if (sn == 0) //逆圆
- {
- if (y >= refrence)
- {
- //第二象限在逆圆情况下不可能2转向1
- if (last != 8)
- quadrant = 7;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在逆圆情况下不可能3转向2
- if (last != 1)
- quadrant = 8;
- else
- quadrant = last;
- }
- }
- else //顺圆
- {
- if (y <= refrence)
- {
- //第二象限在顺圆情况下不可能1转向2
- if (last != 7)
- quadrant = 8;
- else
- quadrant = last;
- }
- else
- {
- //第二象限在顺圆情况下不可能2转向3
- if (last != 6)
- quadrant = 7;
- else
- quadrant = last;
- }
- }
- break;
- }
- return quadrant;
- }
- /**
- * 选择要进行圆弧插补的有效轴
- *
- * @author LXZ (040120)
- *
- * @param interp 插补对象
- * @param axis_x_id X轴编号
- * @param axis_y_id Y轴编号
- */
- void interp_set_arc_axis(interp_task_t *interp, char axis_x_idx, char axis_y_idx) {
- interp->arc_x_idx = axis_x_idx;
- interp->arc_y_idx = axis_y_idx;
- }
- /**
- * 求出两个值的平方差的开方
- *
- * @author LXZ (032820)
- *
- * @param x 圆半径
- * @param y X或者Y坐标相对圆心的偏移量
- * @param ratio1 相对于输入轴的齿轮比
- * @param ratio2 相对于输出轴的齿轮比
- *
- * @return int
- */
- int circle_sqrt(int x, int y, float ratio1, float ratio2) {
- float fx, fy;
- fx = x;
- fy = y / ratio1;
- if (fx <= fy) return 0;
- return (int)(sqrtf((fx * fx - fy * fy)) * ratio2);
- }
- /**
- * 知道中心点在0点的椭圆的一个点一个坐标值,求另一个坐标值
- *
- * @author LXZ (032820)
- *
- * @param x 其中一个坐标值
- * @param a 坐标值对应的轴长
- * @param b 另一个轴的轴长
- *
- * @return int 返回另一个轴的坐标值
- */
- int eclipse_sqrt(float x, float a, float b) {
- //float y = 0;
- //float r = b * b;
- //float res = (x * x) /(a * a) * r;
- //y = sqrtf(r - res);
- //return (int)y;
- return (int)(sqrtf(b * b - (x * x) / (a * a) * (b * b)) + 0.5);
- }
- /**
- * 增加直线路径
- *
- * @author LXZ (032620)
- *
- * @param interp 插补对象
- * @param end_point 终点坐标
- * @param target_speed 目标速度
- * @param stop_speed 停止速度
- */
- void interp_add_liner_path(interp_task_t *interp,
- axis_vector_t *end_point,
- int target_speed,
- int stop_speed
- ) {
- if (interp->path_count < INTERP_PATH_SIZE) { //限制路径条数
- interp_path_t *path = &interp->paths[interp->path_count];
- path->work_type = INTERP_MODE_LINE; //直线插补
- path->end_point = *end_point;
- path->target_speed = target_speed;
- path->stop_speed = stop_speed;
- interp->path_count++;
- }
- }
- /**
- * 增加圆弧路径
- *
- * @author LXZ (032620)
- *
- * @param interp 插补对象
- * @param end_point 终点坐标
- * @param target_speed 目标速度
- * @param stop_speed 停止速度
- */
- void interp_add_arc_path(interp_task_t *interp,
- char arc_x_id,
- char arc_y_id,
- axis_vector_t *end_point,
- axis_vector_t *centre_point,
- int dir,
- int target_speed,
- int stop_speed
- ) {
- if (interp->path_count < INTERP_PATH_SIZE) { //限制路径条数
- interp_path_t *path = &interp->paths[interp->path_count];
- path->work_type = INTERP_MODE_ARC; //圆弧插补
- path->wor_dir = dir;
- path->end_point = *end_point;
- path->centre_point = *centre_point;
- path->target_speed = target_speed;
- path->stop_speed = stop_speed;
- path->arc_x_id = arc_x_id;
- path->arc_y_id = arc_y_id;
- interp->path_count++;
- }
- }
- /**
- * 增加椭圆弧路径
- *
- * @author LXZ (032620)
- *
- * @param interp 插补对象
- * @param end_point 终点坐标
- * @param target_speed 目标速度
- * @param stop_speed 停止速度
- */
- void interp_add_ellipse_path(interp_task_t *interp,
- char arc_x_id,
- char arc_y_id,
- axis_vector_t *end_point,
- axis_vector_t *centre_point,
- int a,
- int b,
- int dir,
- int target_speed,
- int stop_speed
- ) {
- if (interp->path_count < INTERP_PATH_SIZE) { //限制路径条数
- interp_path_t *path = &interp->paths[interp->path_count];
- path->work_type = INTERP_MODE_ELLIPSE; //椭圆弧插补
- path->wor_dir = dir;
- path->end_point = *end_point;
- path->centre_point = *centre_point;
- path->target_speed = target_speed;
- path->stop_speed = stop_speed;
- path->arc_x_id = arc_x_id;
- path->arc_y_id = arc_y_id;
- path->radius_x = a;
- path->radius_y = b;
- interp->path_count++;
- }
- }
- /**
- * 计算圆弧的8分之一参照位置
- *
- * @author LXZ (041022)
- *
- * @param interp 插补对象
- */
- static void interp_calc_arc_refrence(interp_task_t* interp)
- {
- //计算原理
- //要到达8分之一位置 ,需要有X轴走的比例与Y轴走的比例一样
- //假设椭圆长轴的轴半径是A,短轴的轴半径是B
- //即在椭圆中长轴走的距离a与短轴走的距离b关系是,则b = a * B / A;
- //由于椭圆的直线方程为 (X^2)/(A^2)+(Y^2)/(B^2) = 1
- //由于参照点是在椭圆上的,会满足椭圆方程,那么Y=b= a*B/A=X*B/A;
- //则方程可以变化为
- //(X^2)/(A^2) + ((X*B/A)^2)/(B^2) = 1
- //所以求X点坐标可以这么求
- //X =sqrt(1/(1/(A^2) + ((B/A)^2)/B^2))) = sqrt(1/(1/(A^2) + 1/(A^2))) = sqrt((A^2)/2) = A* sqrt(0.5);
- interp->quadrant_refrence.vector[interp->arc_x_idx] = interp->axis_length.vector[interp->arc_x_idx] * sqrt(0.5);
- interp->quadrant_refrence.vector[interp->arc_y_idx] = interp->axis_length.vector[interp->arc_y_idx] * sqrt(0.5);
- }
- /**
- * 通过三点求圆弧的方式,开始插补任务,不满足圆弧时利用直线去走
- *
- * @author LXZ (033120)
- *
- * @param interp 插补对象
- * @param start_point 开始坐标
- * @param end_point 终点坐标
- * @param middle_point 中点坐标
- * @param speed 速度
- */
- void interp_begin_arc_3pt_task(
- interp_task_t *interp,
- axis_vector_t *start_point,
- axis_vector_t *end_point,
- axis_vector_t *middle_point,
- int speed
- ) {
- int i = 0;
- float ax, ay, bx, by, cx, cy;
- float line_flg = 0; // 1, 三点共线
- float mid_a, mid_b, mid_c, mid_d;
- float mid_e, mid_f;
- float x11_org, y11_org;
- float radius;
- ax = start_point->vector[interp->arc_x_idx];
- ay = start_point->vector[interp->arc_y_idx];
- bx = middle_point->vector[interp->arc_x_idx];
- by = middle_point->vector[interp->arc_y_idx];
- cx = end_point->vector[interp->arc_x_idx];
- cy = end_point->vector[interp->arc_y_idx];
- //三点重合,不符合直线
- if (((ax == bx) && (cx == bx)) && ((by == ay) && (cy == by))) {
- interp->mode = INTERP_MODE_FREE;
- return;
- }
- //计算斜率是否一样
- //由公式(y2 - y1)/(x2 - x1) = (y3 - y2) / (x3 - x2)
- //可得变形式 (y2 - y1)* (x3 - x2) - (y3- y2) *(x2 - x1) = 0
- //结果不为0说明不是一条直线,但是可以省掉除法从而提高正确率
- line_flg = (bx - ax) * (cy - by) - (cx - bx) * (by - ay);
- if (line_flg > 0) {
- //该圆为逆时针的圆,因为斜率增大
- interp->arc_dir = 0;
- } else if (line_flg < 0) {
- //该圆为顺圆
- interp->arc_dir = 1;
- } else {
- //这是直线,必须用直线方式运动
- interp_begin_line_task(interp, start_point, end_point, speed, 0);
- return;
- }
- //计算圆心坐标与半径
- //中间计算公式
- mid_a = ax - bx;
- mid_b = ay - by;
- mid_c = ax - cx;
- mid_d = ay - cy;
- mid_e = ((ax * ax - bx * bx) - (by * by - ay * ay)) / 2.0f;
- mid_f = ((ax * ax - cx * cx) - (cy * cy - ay * ay)) / 2.0f;
- //计算(ax - cx) * (ay - by) - (ax - bx) * (ay - cy) != 0
- //同于计算if((ay - cy)/(ax - cx) != (ay - by) / (ax - bx))
- //这个其实是判断三个点的斜率是否有变化,没有任何变化,说明三个点是在一个线上,不可能构成圆
- //通过将除法运算变换成乘法,可以规避除数为0不合法导致异常的事情发生(求斜率经常发生的事)
- if ((mid_b * mid_c - mid_a * mid_d) != 0) {
- //通过圆的方程x*x + y*y = r*r
- //可以分别获得点[a,b]与[a,c]对于圆心(x0,y0)的方程
- //经过不同变换化去x0或者y0可以求出另一个值,公式太麻烦不列出,但是完全可以从圆方程推导出来
- x11_org = -(mid_d * mid_e - mid_b * mid_f) / (mid_b * mid_c - mid_a * mid_d);
- y11_org = -(mid_a * mid_f - mid_c * mid_e) / (mid_b * mid_c - mid_a * mid_d);
- } else {
- interp_begin_line_task(interp, start_point, end_point, speed, 0);
- return;
- }
- //限制有效值是小数点后三位,如果输入是直接脉冲当量,就不例会了
- //if (Math.Abs(x11_org) < 1.0e-3) x11_org = 0.0f;
- //if (Math.Abs(y11_org) < 1.0e-3) y11_org = 0.0f;
- //圆弧需要强制开启X轴与Y轴,暂时不支持Z轴同时插补,所以会关闭Z轴
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- if (i == interp->arc_x_idx ||
- i == interp->arc_y_idx) {
- interp_axis_enable(interp, i);
- } else {
- interp_axis_disable(interp, i);
- }
- }
- interp->mode = INTERP_MODE_ARC;
- interp->start_point = *start_point;
- interp->end_point = *end_point;
- //计算半径
- radius = (float)sqrtf((x11_org - ax) * (x11_org - ax) + (y11_org - ay) * (y11_org - ay));
- //保存圆心坐标
- interp->centre_point.vector[interp->arc_x_idx] = (int)x11_org;
- interp->centre_point.vector[interp->arc_y_idx] = (int)y11_org;
- interp->arc_radius = (int)radius;
- //起点与终点所在的象限
- int quadrant_start = arc_get_quadrant(interp->arc_dir,
- (int)(ax - interp->centre_point.vector[interp->arc_x_idx]),
- (int)(ay - interp->centre_point.vector[interp->arc_y_idx]));
- int quadrant_end = arc_get_quadrant(interp->arc_dir,
- (int)(cx - interp->centre_point.vector[interp->arc_x_idx]),
- (int)(cy - interp->centre_point.vector[interp->arc_y_idx]));
- //计算两轴的总进给
- ax = interp->start_point.vector[interp->arc_x_idx] -
- interp->centre_point.vector[interp->arc_x_idx];
- ay = interp->start_point.vector[interp->arc_y_idx] -
- interp->centre_point.vector[interp->arc_y_idx];
- cx = interp->end_point.vector[interp->arc_x_idx] -
- interp->centre_point.vector[interp->arc_x_idx];
- cy = interp->end_point.vector[interp->arc_y_idx] -
- interp->centre_point.vector[interp->arc_y_idx];
- {
- //求角度,
- float start_deg = (float)(acos((float)ax / interp->arc_radius) * 180 / M_PI);
- float end_deg = (float)(acos((float)cx / interp->arc_radius) * 180 / M_PI);
- float delta_deg = 0;
- if (quadrant_start == 1 && start_deg == 0 && interp->arc_dir == 1) //当判断是第一象限并且是顺针同时也是角度0的时候,
- // 其实它是算第四象限才对
- {
- start_deg = 360;
- } else if (quadrant_start > 2) { //180度修正
- start_deg = 360 - start_deg;
- }
- if ((quadrant_end == 1 && end_deg == 0 && interp->arc_dir == 0)) //当判断是第一象限并且是逆时针同时也是角度0的时候,
- // 其实它是算第四象限才对
- {
- end_deg = 360;
- } else if (quadrant_end > 2) {
- end_deg = 360 - end_deg;
- }
- //初始化总进给量
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- interp->axis[i].total_delta = 0;
- }
- //求总进给角度
- //根据角度求出具体的总进给值
- if (interp->arc_dir == 0) { //逆时针
- delta_deg = end_deg - start_deg;
- if (delta_deg < 0) {
- delta_deg += 360;
- }
- if (start_deg <= 180 && end_deg >= 180) { //从圆左边跨象限
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius + ax) + (interp->arc_radius + cx)));
- } else if (start_deg >= 180 && end_deg <= 180) { //从圆右边跨象限
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius - ax) + (interp->arc_radius - cx)));
- } else if (start_deg >= end_deg) { //大于半圆
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(interp->arc_radius * 4 - abs((int)(cx - ax))));
- } else { //小于半圆
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(cx - ax));
- }
- if ((start_deg <= 90 || start_deg >= 270) && (end_deg <= 270 && end_deg >= 90)) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius - ay) + (interp->arc_radius - cy)));
- } else if ((start_deg >= 90 && start_deg <= 270) && (end_deg <= 90 || end_deg >= 270)) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius + ay) + (interp->arc_radius + cy)));
- } else if (delta_deg > 180) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(4 * interp->arc_radius - abs((int)(cy - ay))));
- } else {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(cy - ay));
- }
- } else { //顺时针
- delta_deg = start_deg - end_deg;
- if (delta_deg < 0) {
- delta_deg += 360;
- }
- if (start_deg <= 180 && end_deg >= 180) { //从圆左边跨象限
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius - ax) + (interp->arc_radius - cx)));
- } else if (start_deg >= 180 && end_deg <= 180) { //从圆右边跨象限
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius + ax) + (interp->arc_radius + cx)));
- } else if (start_deg <= end_deg) { //大于半圆
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(interp->arc_radius * 4 - abs((int)(cx - ax))));
- } else { //小于半圆
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(cx - ax));
- }
- if ((start_deg <= 90 || start_deg >= 270) && (end_deg <= 270 && end_deg >= 90)) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius + ay) + (interp->arc_radius + cy)));
- } else if ((start_deg >= 90 && start_deg <= 270) && (end_deg <= 90 || end_deg >= 270)) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius - ay) + (interp->arc_radius - cy)));
- } else if (delta_deg > 180) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(4 * interp->arc_radius - abs((int)(cy - ay))));
- } else {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(cy - ay));
- }
- }
- }
- //进给量比例变换
- interp->axis[interp->arc_x_idx].total_delta = pi_round(interp->axis[interp->arc_x_idx].total_delta,
- interp->axis[interp->arc_x_idx].ratio);
- interp->axis[interp->arc_y_idx].total_delta = pi_round(interp->axis[interp->arc_y_idx].total_delta,
- interp->axis[interp->arc_y_idx].ratio);
- interp->start_point.vector[interp->arc_x_idx] = pi_round(interp->start_point.vector[interp->arc_x_idx],
- interp->axis[interp->arc_x_idx].ratio);
- interp->start_point.vector[interp->arc_y_idx] = pi_round(interp->start_point.vector[interp->arc_y_idx],
- interp->axis[interp->arc_y_idx].ratio);
- interp->end_point.vector[interp->arc_x_idx] = pi_round(interp->end_point.vector[interp->arc_x_idx],
- interp->axis[interp->arc_x_idx].ratio);
- interp->end_point.vector[interp->arc_y_idx] = pi_round(interp->end_point.vector[interp->arc_y_idx],
- interp->axis[interp->arc_y_idx].ratio);
- interp->centre_point.vector[interp->arc_x_idx] = pi_round(interp->centre_point.vector[interp->arc_x_idx],
- interp->axis[interp->arc_x_idx].ratio);
- interp->centre_point.vector[interp->arc_y_idx] = pi_round(interp->centre_point.vector[interp->arc_y_idx],
- interp->axis[interp->arc_y_idx].ratio);
- interp->axis_length.vector[interp->arc_x_idx] = pi_round(interp->arc_radius, interp->axis[interp->arc_x_idx].ratio);
- interp->axis_length.vector[interp->arc_y_idx] = pi_round(interp->arc_radius, interp->axis[interp->arc_y_idx].ratio);
- //计算8分圆的参考位置(相对圩圆心的绝对偏移)
- interp_calc_arc_refrence(interp);
- //利用三点一计算出圆心与方向
- interp_task_init(interp, speed, 0);
- }
- /**
- * 通过圆心与方向,开始圆弧的插补任务
- *
- * @author LXZ (032620)
- *
- * @param interp 插补对象
- * @param start_point 开始坐标
- * @param end_point 终点坐标
- * @param centre_point 圆心坐标
- * @param dir 方向,0表示逆时针,1表示顺时针
- */
- void interp_begin_arc_task(
- interp_task_t *interp,
- axis_vector_t *start_point,
- axis_vector_t *end_point,
- axis_vector_t *centre_point,
- int dir,
- int speed,
- int stop_speed
- ) {
- float dx, dy, radius;
- int ax, ay, cx, cy;
- int i = 0;
- //求半径
- dx = centre_point->vector[interp->arc_x_idx] -
- start_point->vector[interp->arc_x_idx];
- dy = centre_point->vector[interp->arc_y_idx] -
- start_point->vector[interp->arc_y_idx];
- ax = start_point->vector[interp->arc_x_idx] -
- centre_point->vector[interp->arc_x_idx];
- ay = start_point->vector[interp->arc_y_idx] -
- centre_point->vector[interp->arc_y_idx];
- cx = end_point->vector[interp->arc_x_idx] -
- centre_point->vector[interp->arc_x_idx];
- cy = end_point->vector[interp->arc_y_idx] -
- centre_point->vector[interp->arc_y_idx];
- radius = sqrtf(dx * dx + dy * dy);
- //初始值
- interp->arc_dir = dir;
- interp->centre_point = *centre_point;
- interp->start_point = *start_point;
- interp->end_point = *end_point;
- interp->arc_radius = (int)radius;
- //计算进给总量
- //圆弧需要强制开启X轴与Y轴,暂时不支持Z轴同时插补,所以会关闭Z轴
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- if (i == interp->arc_x_idx ||
- i == interp->arc_y_idx) {
- interp_axis_enable(interp, i);
- } else {
- interp_axis_disable(interp, i);
- }
- }
- interp->mode = INTERP_MODE_ARC;
- if (start_point->vector[interp->arc_x_idx] == end_point->vector[interp->arc_x_idx] &&
- start_point->vector[interp->arc_y_idx] == end_point->vector[interp->arc_y_idx]) {
- interp->axis[interp->arc_x_idx].total_delta = interp->arc_radius * 4;
- interp->axis[interp->arc_y_idx].total_delta = interp->arc_radius * 4;
- } else {
- //起点与终点所在的象限
- int quadrant_start = arc_get_quadrant(interp->arc_dir,
- ax,
- ay);
- int quadrant_end = arc_get_quadrant(interp->arc_dir,
- cx,
- cy);
- //求角度,
- float start_deg = (float)(acosf((float)ax / interp->arc_radius) * 180 / M_PI);
- float end_deg = (float)(acosf((float)cx / interp->arc_radius) * 180 / M_PI);
- float delta_deg = 0;
- //根据象限修正角度
- if (quadrant_start == 1 && start_deg == 0 && interp->arc_dir == 1) //当判断是第一象限并且是顺针同时也是角度0的时候,
- // 其实它是算第四象限才对
- {
- start_deg = 360;
- } else if (quadrant_start > 2) { //180度修正
- start_deg = 360 - start_deg;
- }
- if ((quadrant_end == 1 && end_deg == 0 && interp->arc_dir == 0)) //当判断是第一象限并且是逆时针同时也是角度0的时候,
- // 其实它是算第四象限才对
- {
- end_deg = 360;
- } else if (quadrant_end > 2) {
- end_deg = 360 - end_deg;
- }
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- interp->axis[i].total_delta = 0;
- }
- //根据角度求进给量
- //根据角度求出具体的总进给值
- //根据角度求出具体的总进给值
- if (interp->arc_dir == 0) { //逆时针
- delta_deg = end_deg - start_deg;
- if (delta_deg < 0) {
- delta_deg += 360;
- }
- if (start_deg <= 180 && end_deg >= 180) { //从圆左边跨象限
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius + ax) + (interp->arc_radius + cx)));
- } else if (start_deg >= 180 && end_deg <= 180) { //从圆右边跨象限
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius - ax) + (interp->arc_radius - cx)));
- } else if (start_deg >= end_deg) { //大于半圆
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(interp->arc_radius * 4 - abs((int)(cx - ax))));
- } else { //小于半圆
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(cx - ax));
- }
- if ((start_deg <= 90 || start_deg >= 270) && (end_deg <= 270 && end_deg >= 90)) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius - ay) + (interp->arc_radius - cy)));
- } else if ((start_deg >= 90 && start_deg <= 270) && (end_deg <= 90 || end_deg >= 270)) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius + ay) + (interp->arc_radius + cy)));
- } else if (delta_deg > 180) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(4 * interp->arc_radius - abs((int)(cy - ay))));
- } else {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(cy - ay));
- }
- } else { //顺时针
- delta_deg = start_deg - end_deg;
- if (delta_deg < 0) {
- delta_deg += 360;
- }
- if (start_deg <= 180 && end_deg >= 180) { //从圆左边跨象限
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius - ax) + (interp->arc_radius - cx)));
- } else if (start_deg >= 180 && end_deg <= 180) { //从圆右边跨象限
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius + ax) + (interp->arc_radius + cx)));
- } else if (start_deg <= end_deg) { //大于半圆
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(interp->arc_radius * 4 - abs((int)(cx - ax))));
- } else { //小于半圆
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(cx - ax));
- }
- if ((start_deg <= 90 || start_deg >= 270) && (end_deg <= 270 && end_deg >= 90)) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius + ay) + (interp->arc_radius + cy)));
- } else if ((start_deg >= 90 && start_deg <= 270) && (end_deg <= 90 || end_deg >= 270)) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius - ay) + (interp->arc_radius - cy)));
- } else if (delta_deg > 180) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(4 * interp->arc_radius - abs((int)(cy - ay))));
- } else {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(cy - ay));
- }
- }
- }
- //进给量比例变换
- interp->axis[interp->arc_x_idx].total_delta = pi_round(interp->axis[interp->arc_x_idx].total_delta,
- interp->axis[interp->arc_x_idx].ratio);
- interp->axis[interp->arc_y_idx].total_delta = pi_round(interp->axis[interp->arc_y_idx].total_delta,
- interp->axis[interp->arc_y_idx].ratio);
- interp->start_point.vector[interp->arc_x_idx] = pi_round(interp->start_point.vector[interp->arc_x_idx],
- interp->axis[interp->arc_x_idx].ratio);
- interp->start_point.vector[interp->arc_y_idx] = pi_round(interp->start_point.vector[interp->arc_y_idx],
- interp->axis[interp->arc_y_idx].ratio);
- interp->end_point.vector[interp->arc_x_idx] = pi_round(interp->end_point.vector[interp->arc_x_idx],
- interp->axis[interp->arc_x_idx].ratio);
- interp->end_point.vector[interp->arc_y_idx] = pi_round(interp->end_point.vector[interp->arc_y_idx],
- interp->axis[interp->arc_y_idx].ratio);
- interp->centre_point.vector[interp->arc_x_idx] = pi_round(interp->centre_point.vector[interp->arc_x_idx],
- interp->axis[interp->arc_x_idx].ratio);
- interp->centre_point.vector[interp->arc_y_idx] = pi_round(interp->centre_point.vector[interp->arc_y_idx],
- interp->axis[interp->arc_y_idx].ratio);
- interp->axis_length.vector[interp->arc_x_idx] = (int)pi_round(radius, interp->axis[interp->arc_x_idx].ratio);
- interp->axis_length.vector[interp->arc_y_idx] = (int)pi_round(radius, interp->axis[interp->arc_y_idx].ratio);
- //计算8分圆的参考位置(相对圩圆心的绝对偏移)
- interp_calc_arc_refrence(interp);
- interp_task_init(interp, speed, stop_speed);
- }
- /**
- * 通过椭圆圆心与方向,开始椭圆弧的插补任务
- *
- * @author LXZ (032620)
- *
- * @param interp 插补对象
- * @param start_point 开始坐标
- * @param end_point 终点坐标
- * @param centre_point 圆心坐标
- * @param radius_x 椭圆弧X轴轴长
- * @param radius_y 椭圆弧Y轴轴长
- * @param dir 方向,0表示逆时针,1表示顺时针
- */
- void interp_begin_ellipse_task(
- interp_task_t *interp,
- axis_vector_t *start_point,
- axis_vector_t *end_point,
- axis_vector_t *centre_point,
- float radius_x,
- float radius_y,
- int dir,
- int speed,
- int stop_speed) {
- int ax, ay, cx, cy;
- int i = 0;
- //求半径
- //dx = centre_point->vector[interp->arc_x_idx] -
- // start_point->vector[interp->arc_x_idx];
- //dy = centre_point->vector[interp->arc_y_idx] -
- // start_point->vector[interp->arc_y_idx];
- ax = start_point->vector[interp->arc_x_idx] -
- centre_point->vector[interp->arc_x_idx];
- ay = start_point->vector[interp->arc_y_idx] -
- centre_point->vector[interp->arc_y_idx];
- cx = end_point->vector[interp->arc_x_idx] -
- centre_point->vector[interp->arc_x_idx];
- cy = end_point->vector[interp->arc_y_idx] -
- centre_point->vector[interp->arc_y_idx];
- //初始值
- interp->arc_dir = dir;
- interp->centre_point = *centre_point;
- interp->start_point = *start_point;
- interp->end_point = *end_point;
- //计算进给总量
- //圆弧需要强制开启X轴与Y轴,暂时不支持Z轴同时插补,所以会关闭Z轴
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- if (i == interp->arc_x_idx ||
- i == interp->arc_y_idx) {
- interp_axis_enable(interp, i);
- } else {
- interp_axis_disable(interp, i);
- }
- }
- interp->mode = INTERP_MODE_ARC;
- if (start_point->vector[interp->arc_x_idx] == end_point->vector[interp->arc_x_idx] &&
- start_point->vector[interp->arc_y_idx] == end_point->vector[interp->arc_y_idx]) {
- interp->axis[interp->arc_x_idx].total_delta = (int)(radius_x * 4);
- interp->axis[interp->arc_y_idx].total_delta = (int)(radius_y * 4);
- } else {
- //起点与终点所在的象限
- int quadrant_start = arc_get_quadrant(interp->arc_dir,
- ax,
- ay);
- int quadrant_end = arc_get_quadrant(interp->arc_dir,
- cx,
- cy);
- //求角度,
- float start_deg = (float)(acosf((float)ax / radius_x) * 180 / M_PI);
- float end_deg = (float)(acosf((float)cx / radius_y) * 180 / M_PI);
- float delta_deg = 0;
- //根据象限修正角度
- if (quadrant_start == 1 && start_deg == 0 && interp->arc_dir == 1) //当判断是第一象限并且是顺针同时也是角度0的时候,
- // 其实它是算第四象限才对
- {
- start_deg = 360;
- } else if (quadrant_start > 2) { //180度修正
- start_deg = 360 - start_deg;
- }
- if ((quadrant_end == 1 && end_deg == 0 && interp->arc_dir == 0)) //当判断是第一象限并且是逆时针同时也是角度0的时候,
- // 其实它是算第四象限才对
- {
- end_deg = 360;
- } else if (quadrant_end > 2) {
- end_deg = 360 - end_deg;
- }
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- interp->axis[i].total_delta = 0;
- }
- //根据角度求进给量
- //根据角度求出具体的总进给值
- //根据角度求出具体的总进给值
- if (interp->arc_dir == 0) { //逆时针
- delta_deg = end_deg - start_deg;
- if (delta_deg < 0) {
- delta_deg += 360;
- }
- if (start_deg <= 180 && end_deg >= 180) { //从圆左边跨象限
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((radius_x + ax) + (radius_x + cx)));
- } else if (start_deg >= 180 && end_deg <= 180) { //从圆右边跨象限
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((radius_x - ax) + (radius_x - cx)));
- } else if (start_deg >= end_deg) { //大于半圆
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(radius_x * 4 - abs((int)(cx - ax))));
- } else { //小于半圆
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(cx - ax));
- }
- if ((start_deg <= 90 || start_deg >= 270) && (end_deg <= 270 && end_deg >= 90)) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((radius_y - ay) + (radius_y - cy)));
- } else if ((start_deg >= 90 && start_deg <= 270) && (end_deg <= 90 || end_deg >= 270)) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((radius_y + ay) + (radius_y + cy)));
- } else if (delta_deg > 180) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(4 * radius_y - abs((int)(cy - ay))));
- } else {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(cy - ay));
- }
- } else { //顺时针
- delta_deg = start_deg - end_deg;
- if (delta_deg < 0) {
- delta_deg += 360;
- }
- if (start_deg <= 180 && end_deg >= 180) { //从圆左边跨象限
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((radius_x - ax) + (radius_x - cx)));
- } else if (start_deg >= 180 && end_deg <= 180) { //从圆右边跨象限
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((radius_x + ax) + (radius_x + cx)));
- } else if (start_deg <= end_deg) { //大于半圆
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(radius_x * 4 - abs((int)(cx - ax))));
- } else { //小于半圆
- interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(cx - ax));
- }
- if ((start_deg <= 90 || start_deg >= 270) && (end_deg <= 270 && end_deg >= 90)) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((radius_y + ay) + (radius_y + cy)));
- } else if ((start_deg >= 90 && start_deg <= 270) && (end_deg <= 90 || end_deg >= 270)) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((radius_y - ay) + (radius_y - cy)));
- } else if (delta_deg > 180) {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(4 * radius_y - abs((int)(cy - ay))));
- } else {
- interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(cy - ay));
- }
- }
- }
- //进给量比例变换
- interp->axis[interp->arc_x_idx].total_delta = pi_round(interp->axis[interp->arc_x_idx].total_delta,
- interp->axis[interp->arc_x_idx].ratio);
- interp->axis[interp->arc_y_idx].total_delta = pi_round(interp->axis[interp->arc_y_idx].total_delta,
- interp->axis[interp->arc_y_idx].ratio);
- interp->start_point.vector[interp->arc_x_idx] = pi_round(interp->start_point.vector[interp->arc_x_idx],
- interp->axis[interp->arc_x_idx].ratio);
- interp->start_point.vector[interp->arc_y_idx] = pi_round(interp->start_point.vector[interp->arc_y_idx],
- interp->axis[interp->arc_y_idx].ratio);
- interp->end_point.vector[interp->arc_x_idx] = pi_round(interp->end_point.vector[interp->arc_x_idx],
- interp->axis[interp->arc_x_idx].ratio);
- interp->end_point.vector[interp->arc_y_idx] = pi_round(interp->end_point.vector[interp->arc_y_idx],
- interp->axis[interp->arc_y_idx].ratio);
- interp->centre_point.vector[interp->arc_x_idx] = pi_round(interp->centre_point.vector[interp->arc_x_idx],
- interp->axis[interp->arc_x_idx].ratio);
- interp->centre_point.vector[interp->arc_y_idx] = pi_round(interp->centre_point.vector[interp->arc_y_idx],
- interp->axis[interp->arc_y_idx].ratio);
- interp->axis_length.vector[interp->arc_x_idx] = (int)pi_round(radius_x, interp->axis[interp->arc_x_idx].ratio);
- interp->axis_length.vector[interp->arc_y_idx] = (int)pi_round(radius_y, interp->axis[interp->arc_y_idx].ratio);
- //计算8分圆的参考位置(相对圩圆心的绝对偏移)
- interp_calc_arc_refrence(interp);
- interp_task_init(interp, speed, stop_speed);
- }
- /**
- * 开始直线插补任务,
- *
- * @author LXZ (032620)
- *
- * @param interp 插补对象
- * @param start_point 开始坐标
- * @param end_point 终点坐标
- * @param speed 速度
- */
- void interp_begin_line_task(
- interp_task_t *interp,
- axis_vector_t *start_point,
- axis_vector_t *end_point,
- int speed,
- int stop_speed
- ) {
- int number = 0;
- int i = 0;
- int max_axis_id = 0;
- int max_delta = 0;
- //如果轴无效,就不进入直线插补模式
- //interp->axis[0].total_delta = abs(end_point->X - start_point->X);
- //interp->axis[1].total_delta = abs(end_point->Y - start_point->Y);
- //interp->axis[2].total_delta = abs(end_point->Z - start_point->Z);
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- interp->axis[i].total_delta = (int)(abs(end_point->vector[i] - start_point->vector[i]) * interp->axis[i].ratio);
- if (interp->axis[i].total_delta > 0) {
- interp_axis_enable(interp, i);
- } else {
- interp_axis_disable(interp, i);
- }
- if (interp->axis[i].flg != 0) {
- if (interp->axis[i].total_delta > max_delta) {
- max_delta = interp->axis[i].total_delta;
- max_axis_id = i;
- }
- number++;
- }
- }
- if (number < 1) { //单轴不需要插补
- return;
- }
- interp->line_idx = max_axis_id; //两个轴以上时,最大进给轴就是插补轴
- //interp->axis[max_axis_id].flg += 1; //有两个轴以上的插补,最大进给的轴就是主轴
- interp->mode = INTERP_MODE_LINE; //
- interp->start_point = *start_point;
- interp->end_point = *end_point;
- //坐标比例变换
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- interp->start_point.vector[i] = pi_round(interp->start_point.vector[i], interp->axis[i].ratio);
- interp->end_point.vector[i] = pi_round(interp->end_point.vector[i], interp->axis[i].ratio);
- }
- //初始化轴参数
- interp_task_init(interp, speed, stop_speed);
- }
- /**
- * 圆弧任务
- *
- * @author LXZ (032720)
- *
- * @param interp 插补对象
- * @param result 插补缓冲
- */
- static void interp_work_arc_task(interp_task_t *interp, interp_buffer_t *result) {
- interp_axis_t * axis1,*axis2;
- int cur_quadrant;
- int number = 0;
- int dir = 0;
- int count = result->count;
- int rel_period = interp->ref_clock;
- axis1 = &interp->axis[interp->arc_x_idx];
- axis2 = &interp->axis[interp->arc_y_idx];
- //进来的时候就需要进行一次减速位置初始化
- if (axis1->dec_position == 0) {
- axis1->dec_speed = axis1->stop_speed;
- }
- if (axis2->dec_position == 0) {
- axis2->dec_speed = axis2->stop_speed;
- }
- while (count < INTERP_BUFFER_SIZE &&
- (axis1->cur_delta > 0 || axis2->cur_delta > 0)) {
- //判断当前速度是否能进行加速
- //求当前所在象限
- //进行象限判断需要以轴长为标准
- if (interp->axis_length.vector[axis1->id] >= interp->axis_length.vector[axis2->id])
- {
- //以X轴为长轴进行判断
- cur_quadrant = ellipse_get_eight_of_part_by_xaxis(interp->arc_dir,
- axis1->cur_posi - interp->centre_point.vector[axis1->id],
- axis2->cur_posi - interp->centre_point.vector[axis2->id],
- interp->quadrant_refrence.vector[axis1->id],
- interp->last_quadrant);
- }
- else
- {
- //以Y轴为长轴进行判断
- cur_quadrant = ellipse_get_eight_of_part_by_yaxis(interp->arc_dir,
- axis1->cur_posi - interp->centre_point.vector[axis1->id],
- axis2->cur_posi - interp->centre_point.vector[axis2->id],
- interp->quadrant_refrence.vector[axis2->id],
- interp->last_quadrant);
- }
- //根据象限求出进给表
- switch (cur_quadrant) { //根据8分圆来判断主轴的方向
- case 1:
- case 4:
- case 5:
- case 8:
- //在这个区间里面,由于Y轴进给比Y轴大,因此,需要以Y轴为进给参考
- if (
- //cur_quadrant != interp->last_quadrant
- (interp->last_quadrant != 1) && (interp->last_quadrant != 4) &&
- (interp->last_quadrant != 5) && (interp->last_quadrant != 8)
- ) { //象限变了
- //主轴需要交替,因此需要根据上一次的速度值来重置判断条件
- axis2->cur_speed = 0;
- //查找上一次的速度值
- while (axis2->cur_speed + 1 < axis2->max_speed) {
- if (axis2->speed_table[axis2->cur_speed] >= axis2->last_feed) {
- break;
- }
- axis2->cur_speed++;
- }
- //初始化必须的减速脉冲
- axis2->dec_speed = axis2->stop_speed;
- if (axis2->dec_speed <= axis2->cur_speed) { //当前速度比减速速高时,需要计算减速
- axis2->dec_position = axis2->speed_table[axis2->dec_speed];
- while (((axis2->dec_speed + 1) < axis2->max_speed) &&
- (axis2->speed_table[axis2->dec_speed + 1] <
- axis2->speed_table[axis2->cur_speed])) {
- axis2->dec_speed++;
- axis2->dec_position += axis2->speed_table[axis2->dec_speed];
- }
- } else {
- axis2->dec_position = 0;
- }
- } else if (axis2->cur_delta - axis2->dec_position >
- axis2->speed_table[axis2->cur_speed]) { //当前还允许进行该速度前进
- //先判断减速
- if ((axis2->dec_speed + 1 < axis2->max_speed) &&
- axis2->speed_table[axis2->dec_speed + 1] < axis2->last_feed) {
- if (axis2->cur_delta - axis2->dec_position >
- axis2->speed_table[axis2->dec_speed + 1]) { //找出减速点
- axis2->dec_speed++;
- axis2->dec_position += axis2->speed_table[axis2->dec_speed];
- }
- } else {
- //计算Y轴下次进给值
- number = axis2->speed_table[axis2->cur_speed];
- axis2->last_period = rel_period / number;
- if (axis2->cur_delta < number) {
- number = axis2->cur_delta;
- }
- axis2->cur_delta -= number;
- //根据8分圆计算进给方向
- if (interp->arc_dir == 0) { //逆时针
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 7 ||
- cur_quadrant == 8) {
- dir = 1;
- } else {
- dir = -1;
- }
- } else { //顺时针
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 7 ||
- cur_quadrant == 8) {
- dir = -1;
- } else {
- dir = 1;
- }
- }
- result->feed[count][axis2->id] = number * dir;
- axis2->cur_posi += result->feed[count][axis2->id];
- result->period[count][axis2->id] = axis2->last_period;
- axis2->last_feed = number;
- //计算X轴在Y轴下一个位置下的进给坐标
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 7 ||
- cur_quadrant == 8) {
- number = (int)eclipse_sqrt((axis2->cur_posi - interp->centre_point.vector[axis2->id]),
- interp->axis_length.vector[axis2->id],
- interp->axis_length.vector[axis1->id]) +
- interp->centre_point.vector[axis1->id];
- } else {
- number = -(int)eclipse_sqrt((axis2->cur_posi - interp->centre_point.vector[axis2->id]),
- interp->axis_length.vector[axis2->id],
- interp->axis_length.vector[axis1->id]) +
- interp->centre_point.vector[axis1->id];
- }
- //判断进给方向
- if (number == axis1->cur_posi) {
- if (interp->arc_dir == 0) { //逆时针
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 3 ||
- cur_quadrant == 4) {
- dir = -1;
- } else {
- dir = 1;
- }
- } else { //顺时针
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 3 ||
- cur_quadrant == 4) {
- dir = 1;
- } else {
- dir = -1;
- }
- }
- number = 0;
- } else if (number > axis1->cur_posi) {
- number = number - axis1->cur_posi;
- dir = 1;
- } else {
- number = axis1->cur_posi - number;
- dir = -1;
- }
- if (number != 0 && axis1->cur_delta > 0) {
- axis1->last_period = rel_period / number;
- result->period[count][axis1->id] = axis1->last_period;
- result->feed[count][axis1->id] = number * dir;
- axis1->cur_posi += result->feed[count][axis1->id];
- axis1->last_feed = number;
- if (axis1->cur_delta > number) {
- axis1->cur_delta -= number;
- } else {
- axis1->cur_delta = 0;
- }
- } else {
- result->feed[count][axis1->id] = 0;
- axis1->last_feed = 0;
- axis1->last_period = PERIOD_MAX_VALUE;
- }
- if ((axis2->cur_delta - axis2->dec_position >
- axis2->speed_table[axis2->cur_speed])
- && (axis2->cur_speed + 1) < axis2->max_speed) { //满足加速条件就进行加速
- axis2->cur_speed++;
- }
- INTERP_FEED_DEBUG(++interp->feed_count);
- count++;
- }
- } else {
- //开始执行减速运动
- if (axis2->cur_delta -
- (axis2->dec_position - axis2->speed_table[axis2->dec_speed]) >=
- axis2->speed_table[axis2->dec_speed]) {
- //如果剩余的脉冲是允许进行减速的,就继续在当前速度下进行运动来达到更快到达目的进给
- //Y轴
- number = axis2->speed_table[axis2->dec_speed];
- axis2->last_period = rel_period / number;
- if (axis2->cur_delta < number) {
- number = axis2->cur_delta;
- }
- axis2->cur_delta -= number;
- if (interp->arc_dir == 0) {
- if (cur_quadrant == 3 ||
- cur_quadrant == 4 ||
- cur_quadrant == 5 ||
- cur_quadrant == 6) {
- dir = -1;
- } else {
- dir = 1;
- }
- } else {
- if (cur_quadrant == 3 ||
- cur_quadrant == 4 ||
- cur_quadrant == 5 ||
- cur_quadrant == 6) {
- dir = 1;
- } else {
- dir = -1;
- }
- }
- result->feed[count][axis2->id] = number * dir;
- axis2->cur_posi += result->feed[count][axis2->id];
- result->period[count][axis2->id] = axis2->last_period;
- axis2->last_feed = number;
- //计算X轴在Y轴下一个位置下的进给坐标
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 7 ||
- cur_quadrant == 8) {
- number = (int)eclipse_sqrt((axis2->cur_posi - interp->centre_point.vector[axis2->id]),
- interp->axis_length.vector[axis2->id],
- interp->axis_length.vector[axis1->id]) +
- interp->centre_point.vector[axis1->id];
- } else {
- number = -(int)eclipse_sqrt((axis2->cur_posi - interp->centre_point.vector[axis2->id]),
- interp->axis_length.vector[axis2->id],
- interp->axis_length.vector[axis1->id]) +
- interp->centre_point.vector[axis1->id];
- }
- //判断进给方向
- if (number == axis1->cur_posi) {
- if (interp->arc_dir == 0) { //逆时针
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 3 ||
- cur_quadrant == 4) {
- dir = -1;
- } else {
- dir = 1;
- }
- } else { //顺时针
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 3 ||
- cur_quadrant == 4) {
- dir = 1;
- } else {
- dir = -1;
- }
- }
- number = 0;
- } else if (number > axis1->cur_posi) {
- number = number - axis1->cur_posi;
- dir = 1;
- } else {
- number = axis1->cur_posi - number;
- dir = -1;
- }
- if (number != 0 && axis1->cur_delta > 0) {
- axis1->last_period = rel_period / number;
- result->period[count][axis1->id] = axis1->last_period;
- result->feed[count][axis1->id] = number * dir;
- axis1->cur_posi += result->feed[count][axis1->id];
- axis1->last_feed = number;
- if (axis1->cur_delta > number) {
- axis1->cur_delta -= number;
- } else {
- axis1->cur_delta = 0;
- }
- } else {
- result->feed[count][axis1->id] = 0;
- axis1->last_feed = 0;
- axis1->last_period = PERIOD_MAX_VALUE;
- }
- INTERP_FEED_DEBUG(++interp->feed_count);
- count++;
- } else if (axis2->dec_speed > axis2->stop_speed) {
- axis2->dec_position -= axis2->speed_table[axis2->dec_speed];
- axis2->dec_speed--;
- } else {
- axis2->dec_position = 0;
- }
- }
- break;
- case 2:
- case 3:
- case 6:
- case 7:
- //在这个区间里面,由于X轴进给比Y轴大,因此,需要以X轴为进给参考
- if (
- //cur_quadrant != interp->last_quadrant
- (interp->last_quadrant != 2) && (interp->last_quadrant != 3) &&
- (interp->last_quadrant != 6) && (interp->last_quadrant != 7)
- ) { //象限变了之后,主轴需要交替,因此需要根据上一次的速度值来重置判断条件
- //interp->last_quadrant = cur_quadrant;
- axis1->cur_speed = 0;
- //查找上一次的速度值
- while ((axis1->cur_speed + 1) < axis1->max_speed) {
- if (axis1->speed_table[axis1->cur_speed] >= axis1->last_feed) {
- break;
- }
- axis1->cur_speed++;
- }
- //初始化必须的减速脉冲
- axis1->dec_speed = axis1->stop_speed;
- if (axis1->cur_speed >= axis1->dec_speed) { //当当前速度比减速速高时,需要计算减速
- axis1->dec_position = axis1->speed_table[axis1->dec_speed];
- while (axis1->dec_speed + 1 < axis1->max_speed &&
- axis1->speed_table[axis1->dec_speed + 1] <
- axis1->speed_table[axis1->cur_speed]) {
- axis1->dec_speed++;
- axis1->dec_position += axis1->speed_table[axis1->dec_speed];
- }
- } else {
- axis1->dec_position = 0;
- }
- } else if (axis1->cur_delta - axis1->dec_position > axis1->speed_table[axis1->cur_speed]) {
- if ((axis1->dec_speed + 1 < axis1->max_speed) &&
- axis1->speed_table[axis1->dec_speed + 1] < axis1->last_feed) {
- if (axis1->cur_delta - axis1->dec_position > axis1->speed_table[axis1->dec_speed + 1]) {
- axis1->dec_speed++;
- axis1->dec_position += axis1->speed_table[axis1->dec_speed];
- }
- } else {
- //计算X轴下次进给值
- number = axis1->speed_table[axis1->cur_speed];
- axis1->last_period = rel_period / number;
- if (axis1->cur_delta < number) {
- number = axis1->cur_delta;
- }
- axis1->cur_delta -= number;
- //根据8分圆计算进给方向
- if (interp->arc_dir == 0) { //逆时针
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 3 ||
- cur_quadrant == 4) {
- dir = -1;
- } else {
- dir = 1;
- }
- } else { //顺时针
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 3 ||
- cur_quadrant == 4) {
- dir = 1;
- } else {
- dir = -1;
- }
- }
- result->feed[count][axis1->id] = number * dir;
- axis1->cur_posi += result->feed[count][axis1->id];
- result->period[count][axis1->id] = axis1->last_period;
- axis1->last_feed = number;
- //计算Y轴在X轴下一个位置下的进给坐标
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 3 ||
- cur_quadrant == 4) {
- number = (int)eclipse_sqrt((axis1->cur_posi - interp->centre_point.vector[axis1->id]),
- interp->axis_length.vector[axis1->id],
- interp->axis_length.vector[axis2->id]) +
- interp->centre_point.vector[axis2->id];
- } else {
- number = -(int)eclipse_sqrt((axis1->cur_posi - interp->centre_point.vector[axis1->id]),
- interp->axis_length.vector[axis1->id],
- interp->axis_length.vector[axis2->id]) +
- interp->centre_point.vector[axis2->id];
- }
- //判断进给方向
- if (number == axis2->cur_posi) {
- number = 0;
- if (interp->arc_dir == 0) { //逆时针
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 7 ||
- cur_quadrant == 8) {
- dir = 1;
- } else {
- dir = -1;
- }
- } else { //顺时针
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 7 ||
- cur_quadrant == 8) {
- dir = -1;
- } else {
- dir = 1;
- }
- }
- } else if (number > axis2->cur_posi) {
- number = number - axis2->cur_posi;
- dir = 1;
- } else {
- number = axis2->cur_posi - number;
- dir = -1;
- }
- //计算轴2进给量
- if (number > 0 && axis2->cur_delta > 0) {
- axis2->last_period = rel_period / number;
- result->period[count][axis2->id] = axis2->last_period;
- result->feed[count][axis2->id] = number * dir;
- axis2->cur_posi += result->feed[count][axis2->id];
- axis2->last_feed = number;
- if (axis2->cur_delta > number) {
- axis2->cur_delta -= number;
- } else {
- axis2->cur_delta = 0;
- }
- } else {
- result->feed[count][axis2->id] = 0;
- axis2->last_feed = 0;
- axis2->last_period = PERIOD_MAX_VALUE;
- }
- if ((axis1->cur_delta - axis1->dec_position >
- axis1->speed_table[axis1->cur_speed]) &&
- (axis1->cur_speed + 1) < axis1->max_speed) {
- axis1->cur_speed++;
- }
- INTERP_FEED_DEBUG(++interp->feed_count);
- count++;
- }
- } else {
- //开始执行减速运动
- if (axis1->cur_delta -
- (axis1->dec_position - axis1->speed_table[axis1->dec_speed]) >=
- axis1->speed_table[axis1->dec_speed]) {
- //如果剩余的脉冲是允许进行减速的,就继续在当前速度下进行运动来达到更快到达目的进给
- //计算X轴下次进给值
- number = axis1->speed_table[axis1->dec_speed];
- axis1->last_period = rel_period / number;
- if (axis1->cur_delta < number) {
- number = axis1->cur_delta;
- }
- axis1->cur_delta -= number;
- //根据8分圆计算进给方向
- if (interp->arc_dir == 0) { //逆时针
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 3 ||
- cur_quadrant == 4) {
- dir = -1;
- } else {
- dir = 1;
- }
- } else { //顺时针
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 3 ||
- cur_quadrant == 4) {
- dir = 1;
- } else {
- dir = -1;
- }
- }
- result->feed[count][axis1->id] = number * dir;
- axis1->cur_posi += result->feed[count][axis1->id];
- result->period[count][axis1->id] = axis1->last_period;
- axis1->last_feed = number;
- //计算Y轴在X轴下一个位置下的进给坐标
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 3 ||
- cur_quadrant == 4) {
- number = (int)eclipse_sqrt((axis1->cur_posi - interp->centre_point.vector[axis1->id]),
- interp->axis_length.vector[axis1->id],
- interp->axis_length.vector[axis2->id]) +
- interp->centre_point.vector[axis2->id];
- } else {
- number = -(int)eclipse_sqrt((axis1->cur_posi - interp->centre_point.vector[axis1->id]),
- interp->axis_length.vector[axis1->id],
- interp->axis_length.vector[axis2->id]) +
- interp->centre_point.vector[axis2->id];
- }
- //判断进给方向
- if (number == axis2->cur_posi) {
- number = 0;
- if (interp->arc_dir == 0) { //逆时针
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 7 ||
- cur_quadrant == 8) {
- dir = 1;
- } else {
- dir = -1;
- }
- } else { //顺时针
- if (cur_quadrant == 1 ||
- cur_quadrant == 2 ||
- cur_quadrant == 7 ||
- cur_quadrant == 8) {
- dir = -1;
- } else {
- dir = 1;
- }
- }
- } else if (number > axis2->cur_posi) {
- number = number - axis2->cur_posi;
- dir = 1;
- } else {
- number = axis2->cur_posi - number;
- dir = -1;
- }
- //计算轴2进给量
- if (number > 0 && axis2->cur_delta > 0) {
- axis2->last_period = rel_period / number;
- result->period[count][axis2->id] = axis2->last_period;
- result->feed[count][axis2->id] = number * dir;
- axis2->cur_posi += result->feed[count][axis2->id];
- axis2->last_feed = number;
- if (axis2->cur_delta > number) {
- axis2->cur_delta -= number;
- } else {
- axis2->cur_delta = 0;
- }
- } else {
- result->feed[count][axis2->id] = 0;
- axis2->last_feed = 0;
- axis2->last_period = PERIOD_MAX_VALUE;
- }
- INTERP_FEED_DEBUG(++interp->feed_count);
- count++;
- } else if (axis1->dec_speed > axis1->stop_speed) {
- axis1->dec_position -= axis1->speed_table[axis1->dec_speed];
- axis1->dec_speed--;
- } else {
- axis1->dec_position = 0;
- }
- }
- break;
- }
- interp->last_quadrant = cur_quadrant;
- if (count > 0) {
- //当过程中双方进给都为0,说明运算其实结束了,只是因为计算差异会导致其中一个轴存在无法计算的余数
- if (result->feed[count - 1][axis2->id] == 0 && result->feed[count - 1][axis1->id] == 0) {
- count--;
- axis1->cur_delta = 0;
- axis2->cur_delta = 0;
- break;
- }
- }
- }
- if (axis1->cur_delta == 0 && axis2->cur_delta == 0) { //插补运算完成
- int speed = 0;
- int i = 0;
- interp->mode = INTERP_MODE_NONE;
- //需要计算一次完成后的每个轴当前速度
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- axis2 = &interp->axis[i];
- speed = 0;
- while (axis2->speed_table[speed] < axis2->last_feed) {
- speed++;
- }
- axis2->cur_speed = speed;
- }
- }
- //result->used = 0;
- result->count = count;
- }
- /**
- * 直线插补
- *
- * @author LXZ (032720)
- *
- * @param interp 插补对象
- */
- static void interp_work_line_task(interp_task_t *interp, interp_buffer_t *result) {
- interp_axis_t * axis1,*axis2;
- int number = 0;
- float ratio = 0;
- int count = result->count;
- int i = 0;
- int speed = 0;
- int rel_period = interp->ref_clock;
- axis1 = &interp->axis[interp->line_idx];
- if (axis1->dec_position == 0) {
- axis1->dec_speed = axis1->stop_speed;
- if (axis1->cur_speed >= axis1->dec_speed) {
- axis1->dec_position = axis1->speed_table[axis1->dec_speed];
- }
- }
- while (count < INTERP_BUFFER_SIZE && (axis1->cur_delta > 0)) {
- //判断当前速度是否能进行加速
- if (axis1->cur_delta - axis1->dec_position > axis1->speed_table[axis1->cur_speed]) {
- //计算减速距离
- if (((axis1->dec_speed + 1) < axis1->max_speed) &&
- (axis1->speed_table[axis1->dec_speed + 1] < axis1->last_feed)) { //先计算减速脉冲
- if (axis1->cur_delta - axis1->dec_position >
- axis1->speed_table[axis1->dec_speed + 1]) { //如果允许增加减速
- axis1->dec_speed++;
- axis1->dec_position += axis1->speed_table[axis1->dec_speed];
- }
- } else {
- //获取加速度寄存器值
- number = axis1->speed_table[axis1->cur_speed];
- axis1->last_period = rel_period / number;
- axis1->cur_delta -= number;
- axis1->last_feed = number;
- if (axis1->start_posi > axis1->target_posi) {
- axis1->cur_posi -= number;
- result->feed[count][axis1->id] = -number;
- //计算总进度比例
- ratio = (float)(axis1->start_posi - axis1->cur_posi) / axis1->total_delta;
- } else {
- axis1->cur_posi += number;
- result->feed[count][axis1->id] = number;
- //计算总进度比例
- ratio = (float)(axis1->cur_posi - axis1->start_posi) / axis1->total_delta;
- }
- result->period[count][axis1->id] = axis1->last_period;
- //计算总进度比例
- //计算次轴的跟随进给
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- if (i != interp->line_idx) { //非主轴对象
- axis2 = &interp->axis[i];
- number = 0;
- if (axis2->flg != 0) { //选中为插补轴了
- if (axis2->total_delta == axis1->total_delta) { //1比1时不需要计算
- number = axis1->speed_table[axis1->cur_speed];
- } else {
- if (axis2->start_posi > axis2->target_posi) {
- number = (int)(axis2->total_delta * ratio) - (axis2->start_posi - axis2->cur_posi);
- } else {
- number = (int)(axis2->total_delta * ratio) - (axis2->cur_posi - axis2->start_posi);
- }
- }
- }
- if (number > 0) {
- axis2->last_period = rel_period / number;
- if (axis2->start_posi > axis2->target_posi) {
- axis2->cur_posi -= number;
- result->feed[count][axis2->id] = -number;
- } else {
- axis2->cur_posi += number;
- result->feed[count][axis2->id] = number;
- }
- axis2->cur_delta -= number;
- result->period[count][axis2->id] = axis2->last_period;
- axis2->last_feed = number;
- } else {
- axis2->last_period = PERIOD_MAX_VALUE;
- result->feed[count][axis2->id] = 0;
- axis2->last_feed = 0;
- result->period[count][axis2->id] = PERIOD_MAX_VALUE;
- }
- }
- }
- //如果有条件加速就进行加速
- if (axis1->cur_delta - axis1->dec_position >
- axis1->speed_table[axis1->cur_speed] &&
- (axis1->cur_speed + 1) < axis1->max_speed) axis1->cur_speed++;
- INTERP_FEED_DEBUG(++interp->feed_count);
- count++;
- }
- } else {
- //开始执行减速运动
- if (axis1->cur_delta -
- (axis1->dec_position - axis1->speed_table[axis1->dec_speed]) >=
- axis1->speed_table[axis1->dec_speed]) {
- number = axis1->speed_table[axis1->dec_speed];
- axis1->last_period = rel_period / number;
- axis1->cur_delta -= number;
- axis1->last_feed = number;
- if (axis1->start_posi > axis1->target_posi) {
- axis1->cur_posi -= number;
- result->feed[count][axis1->id] = -number;
- //计算总进度比例
- ratio = (float)(axis1->start_posi - axis1->cur_posi) / axis1->total_delta;
- } else {
- axis1->cur_posi += number;
- result->feed[count][axis1->id] = number;
- //计算总进度比例
- ratio = (float)(axis1->cur_posi - axis1->start_posi) / axis1->total_delta;
- }
- result->period[count][axis1->id] = axis1->last_period;
- //计算次轴的跟随进给
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- if (i != interp->line_idx) { //非主轴对象
- axis2 = &interp->axis[i];
- number = 0;
- if (axis2->flg != 0) { //选中为插补轴了
- if (axis2->total_delta == axis1->total_delta) { //1比1时不需要计算
- number = axis1->speed_table[axis1->dec_speed];
- } else {
- if (axis2->start_posi > axis2->target_posi) {
- number = (int)(axis2->total_delta * ratio) - (axis2->start_posi - axis2->cur_posi);
- } else {
- number = (int)(axis2->total_delta * ratio) - (axis2->cur_posi - axis2->start_posi);
- }
- }
- }
- if (number > 0) {
- axis2->last_period = rel_period / number;
- if (axis2->start_posi > axis2->target_posi) {
- axis2->cur_posi -= number;
- result->feed[count][axis2->id] = -number;
- } else {
- axis2->cur_posi += number;
- result->feed[count][axis2->id] = number;
- }
- axis2->cur_delta -= number;
- result->period[count][axis2->id] = axis2->last_period;
- axis2->last_feed = number;
- } else {
- axis2->last_period = PERIOD_MAX_VALUE;
- result->feed[count][axis2->id] = 0;
- axis2->last_feed = 0;
- result->period[count][axis2->id] = PERIOD_MAX_VALUE;
- }
- }
- }
- INTERP_FEED_DEBUG(++interp->feed_count);
- count++;
- } else if (axis1->dec_speed > 0) {
- axis1->dec_position -= axis1->speed_table[axis1->dec_speed];
- axis1->dec_speed--;
- } else {
- axis1->dec_position = 0;
- }
- }
- }
- if (axis1->cur_delta == 0) {
- interp->mode = INTERP_MODE_NONE;
- axis1->cur_speed = axis1->dec_speed;
- //需要计算一次完成后的每个轴当前速度
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- if (i != interp->line_idx) { //非主轴对象
- axis2 = &interp->axis[i];
- speed = 0;
- while (axis2->speed_table[speed] < axis2->last_feed) {
- speed++;
- }
- axis2->cur_speed = speed;
- }
- }
- }
- result->count = count;
- //result->used = 0;
- }
- /**
- * 插补减速停止
- *
- * @author LXZ (033120)
- *
- * @param interp 插补对象
- */
- void interp_task_stop(interp_task_t *interp) {
- interp->stop_flg = 1; //打开了停止位,周期进给会停止
- }
- /**
- * 执行插补任务
- *
- * @author LXZ (032620)
- *
- * @param interp 插补对象
- */
- void interp_task_run(interp_task_t *interp) {
- interp_buffer_t *buffer = (void *)0;
- int i = 0;
- if (!interp_is_fin(interp) && interp->stop_flg == 0) { //有任务并且没有打开停止位时,开始进给
- //查找一个空的可用缓冲
- for (i = 0; i < INTERP_WORK_BUFFER_NUMBER; i++) {
- if (interp->work_buffer[i].count <= interp->work_buffer[i].used) {
- buffer = &interp->work_buffer[i];
- memset(buffer, 0, sizeof(interp_buffer_t));
- interp->work_buffer[i].used = INTERP_BUFFER_SIZE + 1;
- interp->work_buffer[i].count = 0;
- break;
- }
- }
- if (buffer != (void *)0) { //取得有效插补缓冲
- do {
- if (interp->mode == INTERP_MODE_ARC) { //圆弧插补
- interp_work_arc_task(interp, buffer);
- } else if (interp->mode == INTERP_MODE_LINE) { //直线插补
- interp_work_line_task(interp, buffer);
- } else if (interp->path_count > interp->cur_path_index) { //查找下一个路径
- interp_path_t *path = &interp->paths[interp->cur_path_index];
- int i = 0;
- axis_vector_t sp = { 0 };
- axis_vector_t ep = { 0 };
- axis_vector_t cp = { 0 };
- #if 0
- if (interp->cur_path_index > 0) {
- interp_path_t *path0 = &interp->paths[interp->cur_path_index - 1];
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- //修正目标位置,由于上次运算在计算圆弧的时候会引入误差,在运算完后需要修正
- ep.vector[i] = path->end_point.vector[i] - path0->end_point.vector[i] +
- interp->axis[i].cur_posi - interp->axis[i].target_posi;
- cp.vector[i] = path->centre_point.vector[i] - path0->end_point.vector[i] +
- interp->axis[i].cur_posi - interp->axis[i].target_posi;
- }
- } else {
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- ep.vector[i] = path->end_point.vector[i];
- cp.vector[i] = path->centre_point.vector[i];
- }
- }
- #else
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- ep.vector[i] = path->end_point.vector[i];
- cp.vector[i] = path->centre_point.vector[i];
- }
- #endif
- if (path->work_type == INTERP_MODE_LINE) { //直线插补
- //开始直线任务
- interp_begin_line_task(interp, &sp, &ep, path->target_speed, path->stop_speed);
- } else if (path->work_type == INTERP_MODE_ARC) { //圆弧插补
- //设置选用轴
- interp_set_arc_axis(interp, path->arc_x_id, path->arc_y_id);
- //开始圆弧任务
- interp_begin_arc_task(interp, &sp, &ep, &cp,
- path->wor_dir, path->target_speed, path->stop_speed);
- } else if (path->work_type == INTERP_MODE_ELLIPSE) {
- //设置选用轴
- interp_set_arc_axis(interp, path->arc_x_id, path->arc_y_id);
- //开始椭圆弧任务
- interp_begin_ellipse_task(interp, &sp, &ep, &cp,
- path->radius_x, path->radius_y,
- path->wor_dir, path->target_speed, path->stop_speed);
- }
- interp->cur_path_index++;
- } else {
- break;
- }
- } while (buffer->count < INTERP_BUFFER_SIZE);
- if (buffer->count > 0) {
- buffer->used = 0; //使能该缓冲输出
- }
- }
- }
- }
- /**
- * 插补周期运行函数,需要定时调用
- *
- * @author LXZ (032620)
- *
- * @param interp 插补对象
- */
- void interp_task_period(interp_task_t *interp) {
- int i = 0;
- if (interp->stop_flg == 0) {
- interp_buffer_t *buffer = &interp->work_buffer[interp->cur_buffer_index];
- if (buffer->used < buffer->count) {
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) {
- if (buffer->feed[buffer->used][i] != 0) { //插补数据有效时输出到物理轴
- interp->axis[i].real_posi += buffer->feed[buffer->used][i]; //计算新的真实单位位置
- interp->axis[i].cur_feed = abs(buffer->feed[buffer->used][i]);
- interp_axis_period(&interp->axis[i], buffer->feed[buffer->used][i],
- INTERP_REAL_PERIOD(buffer->period[buffer->used][i]));
- } else {
- interp->axis[i].cur_feed = 0;
- }
- }
- INTERP_PERIOD_REPORT(buffer);
- buffer->used++;
- if (buffer->used >= buffer->count) { //插补缓冲完成了
- buffer->count = 0;
- buffer->used = 0;
- if (++interp->cur_buffer_index >= INTERP_WORK_BUFFER_NUMBER) { //切换缓冲
- interp->cur_buffer_index = 0;
- }
- }
- }
- }
- }
- /**
- * 返回插补动作是否完成
- *
- * @author LXZ (032620)
- *
- * @param interp 插补对象
- *
- * @return int 返回0 表示未完成,返回1表示完成
- */
- int interp_is_fin(interp_task_t *interp) {
- int i = 0;
- if (interp->path_count > 0 && interp->cur_path_index < interp->path_count) //路径缓冲池没完
- return 0;
- for (i = 0; i < INTERP_AXIS_NUMBER; i++) { //轴还在运行
- if (interp_axis_is_runnging(&interp->axis[i])) {
- return 0;
- }
- }
- for (i = 0; i < INTERP_DEC_BUFFER_NUMBER; i++) { //插补缓冲还没执行完
- if (interp->work_buffer[i].count > interp->work_buffer[i].used) {
- return 0;
- }
- }
- //当前运算任务也已经完成
- return interp->mode == INTERP_MODE_NONE;
- }
|