12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793 |
- #include "global.h"
- #if SHOU_JIA_MACHINE == 1
- void SJ_CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag);
- void SJ_CTHL_ManualAction(void);
- void SJ_CTHL_AutoStepAction(void);
- void SJ_CTHL_TableAction(void);
- void SJ_CTHL_TD_Motor(void);
- void SJ_CTHL_FZ_Motor(void);
- void SJ_CTHL_CheckStart(void);
- void SJ_CTHL_TLiao_Step(void); //推料(推拉头)
- void SJ_CTHL_AlarmProtect(void);
- void SJ_CTHL_ZLT_Step(void);
- void SJ_CTHL_ZhenDongAction(void);
- void SJ_CTHL_bFKCXTDDWStep(void);
- void SJ_CTHL_bCXCRStep(void);
- void SJ_CTHL_bCLFirstStep(void);
- void ExtiAcitionX02(void)
- {
- if(bCheckEN_FLG)
- {
- bCheckEN_FLG = 0;
- SJ_CTHL_CheckInX02PosBuff = GetPos(X_AXIS);
- }
- }
- //故障报警
- void SJ_CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag)
- {
- SetAlarmCode(SJ_CTHL_ALARM_ADDR,alarm_code);
- // bAlarmStop = 0;
- // if(cStopFlag)
- if(!cWULATOUCnt)
- {
- bAlarmStop = 1;
- }
- else
- {
- cWULATOUCnt=0;
- }
- }
- //初始化动作
- void SJ_CTHL_InitAction(void)
- {
- float buff_pulse,buff_dist;
-
- buff_pulse = SJ_CTHL_PARAM_TDCYCLE_PULSE;
- buff_dist = SJ_CTHL_PARAM_TDCYCLE_LENGTH;
-
- XGearRatio = buff_pulse/buff_dist;
-
- buff_pulse = SJ_CTHL_PARAM_FZCYCLE_PULSE;
- buff_dist = SJ_CTHL_PARAM_FZCYCLE_LENGTH;
- YGearRatio = buff_pulse/buff_dist;
-
-
-
- user_datas[121] = 0;
- user_datas[122] = 0;
- user_datas[123] = 0;
-
- SetEnReverse(X_AXIS, 0);
- SetEnReverse(Y_AXIS, 0);
- SetEnReverse(Z_AXIS, 0);
-
- SetDirReverse(X_AXIS, 1);
- SetDirReverse(Y_AXIS, 0);
- SetDirReverse(Z_AXIS, 0);
-
- SetEn(X_AXIS, MOTOR_DISEN);
- SetEn(Y_AXIS, MOTOR_DISEN);
- SetEn(Z_AXIS, MOTOR_DISEN);
- SetPos(X_AXIS, 0); //启动位置设为0点
- }
- void SJ_CTHL_Action(void)
- {
- // user_datas[121] = SJ_CTHL_LianLengthCheck;
- // user_datas[122] =axis_x->cur_speed;
- // user_datas[123] = SJ_CTHL_bCXCR_Step;
- dwXRealPos = GetPos(X_AXIS);
- dwYRealPos = GetPos(Y_AXIS);
- dwZRealPos = GetPos(Z_AXIS);
- user_datas[121] = dwXRealPos;
- user_datas[122] = dwYRealPos;
-
- // user_datas[126] = SJ_CTHL_TD_MotorSte;//SJ_CTHL_FZ_MotorStep;
- user_datas[127] = SJ_CTHL_LianLengthCheck;
- user_datas[128] = SJ_CTHL_AutoStep;
-
- SJ_CTHL_ZhenDongAction();
- SJ_CTHL_AlarmProtect();
- SJ_CTHL_CheckStart();
- SJ_CTHL_ZLT_Step();
- SJ_CTHL_TLiao_Step();
- SJ_CTHL_TD_Motor();
- SJ_CTHL_FZ_Motor();
- SJ_CTHL_bFKCXTDDWStep();
- SJ_CTHL_bCXCRStep();
- SJ_CTHL_ManualAction();
- SJ_CTHL_AutoStepAction();
- }
- void SJ_CTHL_AlarmProtect(void)
- {
-
-
- }
- //自动动作
- void SJ_CTHL_AutoStepAction(void)
- {
- //测试用
- if(dwTickCount >= SJ_CTHL_TestDelay)
- {
- SJ_CTHL_TestDelay = dwTickCount + 150;
- SJ_CTHL_TestLED = !SJ_CTHL_TestLED;
- }
- if(bRunning)
- {
- switch(SJ_CTHL_AutoStep)
- {
- case 1:
- /*
- SJ_CTHL_AutoStep = 10; //测试拖带定位
- SJ_CTHL_AutoDelay = dwTickCount + 1000;
- break;
- */
- if(dwTickCount >= SJ_CTHL_AutoDelay)
- {
- SJ_CTHL_FKGD_VAVLE = 0; //方块固定关
- SJ_CTHL_YADAI_VAVLE = 1; //过链压带
- SJ_CTHL_LYLun_VAVLE = 0; //Y15方块压轮
- SJ_CTHL_RYLun_VAVLE = 0; //Y16插销压轮
- SJ_CTHL_DXK_VAVLE = 0; //Y01顶斜口电磁阀
- SJ_CTHL_XKDW_VAVLE = 0; //Y02斜口定位(定位上止)
- SJ_CTHL_FWCX_VAVLE = 0; //Y03复位插销
- SJ_CTHL_BAIBI_VAVLE = 0; //Y04摆臂
- SJ_CTHL_AutoStep = 2;
- SJ_CTHL_AutoDelay = dwTickCount + 120;
- if(SJ_CTHL_ZhuangLiaoStep == 0)
- {
- SJ_CTHL_ZhuangLiaoStep = 1;
- SJ_CTHL_TryCnt = 0;
- }
- }
- break;
- case 2:
- if(dwTickCount >= SJ_CTHL_AutoDelay) //压带后才开始拖带
- {
- SJ_CTHL_AutoStep = 3;
- SJ_CTHL_AutoDelay = dwTickCount + 0;
-
- }
- break;
-
- case 3://斜口定位
- if(dwTickCount >= SJ_CTHL_AutoDelay)
- {
- SJ_CTHL_AutoStep = 4;
- SJ_CTHL_AutoDelay = dwTickCount + 0;//SJ_CTHL_PARAM_XKDW_TD_DELAY;
- }
- break;
-
- case 4:
- if(dwTickCount >= SJ_CTHL_AutoDelay)
- {
-
- //调试模式
- if((!SJ_CTHL_bTiaoShiMode) || SJ_CTHL_bTiaoShiStep)
- {
- SJ_CTHL_bTiaoShiStep = 0;
- if(SJ_CTHL_cZipCnt == 0)
- {
- SJ_CTHL_TD_MotorStep = 1; //合链定位 只感应一次
- SJ_CTHL_AutoStep = 5;
- }
- else
- {
- SJ_CTHL_TD_MotorStep = 10; //数控穿入, 感应两次定位
- SJ_CTHL_AutoStep = 5;
- }
- }
- }
- break;
- case 5:
- if(SJ_CTHL_TD_MotorStep == 0) //方块定位完成,下模上去准备斜口拉头穿入
- {
- //装料已经完成
- if((SJ_CTHL_ZhuangLiaoStep == 0) && bZhuangLiaoOkFlg)
- {
- //调试模式
- if((!SJ_CTHL_bTiaoShiMode) || SJ_CTHL_bTiaoShiStep)
- {
- SJ_CTHL_bTiaoShiStep = 0;
- SJ_CTHL_AutoStep = 6;
- SJ_CTHL_CTXM_VAVLE = 1; //下模合上
- SJ_CTHL_AutoDelay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME;
- SJ_CTHL_LYLun_VAVLE = 1; //Y15方块压轮
- SJ_CTHL_RYLun_VAVLE = 1; //Y16插销压轮
- }
- }
-
-
- }
- break;
-
- case 6:
- if(!SJ_CTHL_CTM_Origin_IN)
- SJ_CTHL_JLTou_VAVLE = 0; //Y11接拉头关
- if(SJ_CTHL_CTM_Limit_IN && !SJ_CTHL_CTM_Origin_IN) //穿头模上升到位
- {
- //调试模式
- if((!SJ_CTHL_bTiaoShiMode) || SJ_CTHL_bTiaoShiStep)
- {
- SJ_CTHL_bTiaoShiStep = 0;
- SJ_CTHL_AutoStep = 7;
- SJ_CTHL_XKDW_VAVLE = 1; //Y02斜口定位(定位上止)
- SJ_CTHL_YADAI_VAVLE = 0; //过链压带
- SJ_CTHL_AutoDelay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME;
- }
-
- }
- else if(dwTickCount >= SJ_CTHL_AutoDelay)
- {
- SJ_CTHL_SetAlarmCode(SJ_CTHL_CTM_Limit_ALARM,bRunning); //穿头下模上升异常
- }
- break;
- case 7:
- if(SJ_CTHL_FKSC_DW_IN) //Y02斜口定位(定位上止)
- {
- SJ_CTHL_AutoStep = 8;
- SJ_CTHL_AutoDelay = dwTickCount + SJ_CTHL_PARAM_FKSC_DW_DELAY;//方块上止定位输出后电机延时反转定位
- }
- else if(dwTickCount >= SJ_CTHL_AutoDelay)
- {
- SJ_CTHL_SetAlarmCode(SJ_CTHL_XKTD_Origin_ALARM,bRunning); //斜口推带回到异常
- }
- break;
-
-
- case 8:
- if(dwTickCount >= SJ_CTHL_AutoDelay) //延时电机延时反转定位(也等压轮压到位)
- {
- //调试模式
- if((!SJ_CTHL_bTiaoShiMode) || SJ_CTHL_bTiaoShiStep)
- {
- SJ_CTHL_bTiaoShiStep = 0;
- SJ_CTHL_AutoStep = 9;
- if(SJ_CTHL_PARAM_FKSC_DW_DELAY)
- {
- AxisMovePosAccDec(Y_AXIS,2600,SJ_CTHL_PARAM_FKSC_DW_DELAY,800,500,6,6,0);
- if(SJ_CTHL_PARAM_FKSC_DW_DELAY > 10)
- AxisMovePosAccDec(X_AXIS,2600,-(SJ_CTHL_PARAM_FKSC_DW_DELAY-10),800,500,6,6,0);
- }
- }
- }
- break;
- case 9:
- if(!Y_DRV && !X_DRV) //方块上止定位完成
- {
- SJ_CTHL_AutoStep = 10;
- SJ_CTHL_AutoDelay = dwTickCount + SJ_CTHL_PARAM_DXK_DELAY;
- }
- break;
- case 10:
- if((dwTickCount >= SJ_CTHL_AutoDelay) && !X_DRV)
- {
- //调试模式
- if((!SJ_CTHL_bTiaoShiMode) || SJ_CTHL_bTiaoShiStep)
- {
- SJ_CTHL_bTiaoShiStep = 0;
- SJ_CTHL_DXK_VAVLE = 1; //Y01顶斜口电磁阀
- SJ_CTHL_AutoStep = 11;
- SJ_CTHL_AutoDelay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME;
- }
- }
- break;
- case 11:
- if(SJ_CTHL_XKTDW)
- {
- SJ_CTHL_AutoStep = 12;
- SJ_CTHL_XKDW_VAVLE = 0;
- SJ_CTHL_AutoDelay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME;
-
- }
- else if(dwTickCount >= SJ_CTHL_AutoDelay) //顶斜口到位异常
- {
-
- }
- break;
- case 12:
- if(!SJ_CTHL_FKSC_DW_IN)
- {
- SJ_CTHL_AutoStep = 13;
- SJ_CTHL_AutoDelay = dwTickCount + SJ_CTHL_PARAM_XK_CR_DELAY;//顶斜口后延时穿入
- }
- else if(dwTickCount >= SJ_CTHL_AutoDelay) //退方块上止离开异常
- {
-
- }
- break;
- case 13:
- if(dwTickCount >= SJ_CTHL_AutoDelay)
- {
- //调试模式
- if((!SJ_CTHL_bTiaoShiMode) || SJ_CTHL_bTiaoShiStep)
- {
- SJ_CTHL_bTiaoShiStep = 0;
- SJ_CTHL_AutoStep = 14;
- // if(SJ_CTHL_bFZ_LengthOK)
- // SJ_CTHL_FZ_MotorStep = 10;
- // else
- SJ_CTHL_FZ_MotorStep = 1;
- }
- }
- break;
-
- case 14:
-
- if(SJ_CTHL_FZ_MotorStep == 0) //穿入定位完成
- {
- /* //测试拖带定位
- SJ_CTHL_AutoStep = 1;
- SJ_CTHL_AutoDelay = dwTickCount + 1000;
- if(SJ_CTHL_cZipCnt<3)
- SJ_CTHL_cZipCnt++;
- break; */
- SJ_CTHL_AutoDelay = dwTickCount + SJ_CTHL_PARAM_GDFK_DELAY;
- SJ_CTHL_AutoStep = 15;
- }
- break;
- case 15:
- if(dwTickCount >= SJ_CTHL_AutoDelay)
- {
-
-
- //调试模式
- if((!SJ_CTHL_bTiaoShiMode) || SJ_CTHL_bTiaoShiStep)
- {
- SJ_CTHL_bTiaoShiStep = 0;
-
- SJ_CTHL_FKGD_VAVLE = 1; //Y00方块固定
-
-
- SJ_CTHL_AutoStep = 16;
- // AxisMovePosAccDec(X_AXIS,2000,-(SJ_CTHL_PARAM_CXDW_Length + 250),1000,500,6,6,50);
-
- SJ_CTHL_AutoDelay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME;
- }
- }
- break;
- case 16:
- if(SJ_CTHL_FKGD_DW_IN)
- {
- //调试模式
- if((!SJ_CTHL_bTiaoShiMode) || SJ_CTHL_bTiaoShiStep)
- {
- SJ_CTHL_bTiaoShiStep = 0;
- SJ_CTHL_AutoStep = 17;
- SJ_CTHL_LYLun_VAVLE = 0; //Y15方块压轮
- SJ_CTHL_AutoDelay = dwTickCount + SJ_CTHL_PARAM_DWCX_DELAY;
- }
- }
- else if(dwTickCount >= SJ_CTHL_AutoDelay) //Y00方块固定到位异常
- {
-
- }
- break;
- case 17:
- if(dwTickCount >= SJ_CTHL_AutoDelay) //延时定位插销
- {
- SJ_CTHL_AutoStep = 18;
- SJ_CTHL_FZ_MotorStep = 20; //定位插销启动
-
- }
- break;
- case 18:
- if(SJ_CTHL_FZ_MotorStep == 0)
- {
- SJ_CTHL_YADAI_VAVLE = 1; //Y06压带
- SJ_CTHL_AutoDelay = dwTickCount + SJ_CTHL_PARAM_BAIBI_DELAY;//延时摆臂
- //调试模式
- if((!SJ_CTHL_bTiaoShiMode) || SJ_CTHL_bTiaoShiStep)
- {
- SJ_CTHL_bTiaoShiStep = 0;
- SJ_CTHL_AutoStep = 19;
- }
-
- }
- break;
- case 19:
- if(dwTickCount >= SJ_CTHL_AutoDelay)
- {
- SJ_CTHL_AutoStep = 20;
- SJ_CTHL_BAIBI_VAVLE = 1; //摆臂
- SJ_CTHL_RYLun_VAVLE = 0; //Y16插销压轮
- SJ_CTHL_AutoDelay = dwTickCount + SJ_CTHL_PARAM_CXCR_DELAY; //延时穿插销
- }
- break;
- case 20:
- if(dwTickCount >= SJ_CTHL_AutoDelay)
- {
- //调试模式
- if((!SJ_CTHL_bTiaoShiMode) || SJ_CTHL_bTiaoShiStep)
- {
- SJ_CTHL_bTiaoShiStep = 0;
- SJ_CTHL_AutoStep = 21;
- AxisMovePosAccDec(X_AXIS,3000,(SJ_CTHL_PARAM_XXCR_LENTH),1000,500,6,6,50);
- }
- // SJ_CTHL_XSavePosBuff = dwXRealPos + SJ_CTHL_PARAM_CXDW_Length + 120;
- }
- break;
- case 21:
- // if((dwXRealPos+ 150)>= SJ_CTHL_XSavePosBuff)
- {
-
- }
- if(!X_DRV)// && SJ_CTHL_FWCX_VAVLE)
- {
- //调试模式
- if((!SJ_CTHL_bTiaoShiMode) || SJ_CTHL_bTiaoShiStep)
- {
- SJ_CTHL_bTiaoShiStep = 0;
- SJ_CTHL_AutoStep = 22;
- SJ_CTHL_FWCX_VAVLE = 1;
- SJ_CTHL_AutoDelay = dwTickCount + SJ_CTHL_PARAM_CXCR_O_Time; //延时退方块固定
-
- }
- }
- break;
- case 22:
- if(dwTickCount >= SJ_CTHL_AutoDelay)
- {
- //调试模式
- if((!SJ_CTHL_bTiaoShiMode) || SJ_CTHL_bTiaoShiStep)
- {
- SJ_CTHL_bTiaoShiStep = 0;
- SJ_CTHL_AutoStep = 23;
- SJ_CTHL_FKGD_VAVLE = 0; //Y00方块固定
- SJ_CTHL_AutoDelay = dwTickCount + 15;
- }
- }
- if(SJ_CTHL_XCFW_DW_IN)
- { //插销穿入异常
- SJ_CTHL_AutoStep = 0;
- bRunning = 0;
- bSingOneFlg = 0;
- SJ_CTHL_SetAlarmCode(SJ_CTHL_XCFW_DW_ALARM,bRunning);
- }
- break;
- case 23:
- if(!SJ_CTHL_FKGD_DW_IN)
- {
- //调试模式
- if((!SJ_CTHL_bTiaoShiMode) || SJ_CTHL_bTiaoShiStep)
- {
-
- if(dwTickCount >= SJ_CTHL_AutoDelay)
- {
- SJ_CTHL_bTiaoShiStep = 0;
- SJ_CTHL_TD_MotorStep = 10; //
- SJ_CTHL_AutoStep = 24;
- }
- }
- }
- else
- {
- SJ_CTHL_AutoDelay = dwTickCount + 5;
- if(SJ_CTHL_XCFW_DW_IN)
- { //插销穿入异常
- SJ_CTHL_AutoStep = 0;
- bRunning = 0;
- bSingOneFlg = 0;
- SJ_CTHL_SetAlarmCode(SJ_CTHL_XCFW_DW_ALARM,bRunning);
- }
- }
-
- break;
- case 24:
- if(dwXRealPos > 30) //穿入2CM后退
- {
- SJ_CTHL_AutoStep = 25;
- SJ_CTHL_MGuo_VAVLE = 0; //码勾退
-
- SJ_CTHL_BAIBI_VAVLE = 0;
- SJ_CTHL_AutoDelay = dwTickCount + 8; //延时穿插销
-
- bZhuangLiaoOkFlg = 0;
-
- }
- break;
- case 25:
- if(dwTickCount >= SJ_CTHL_AutoDelay)
- {
- SJ_CTHL_AutoStep = 26;
- SJ_CTHL_CTXM_VAVLE = 0; //下模打开
- //退下出错警告
- SJ_CTHL_AutoDelay1 = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME;
- SJ_CTHL_AutoStep1 = 1;
- }
- break;
- case 26:
- if((SJ_CTHL_TD_MotorStep == 0) && (!SJ_CTHL_CTXM_VAVLE ))
- {
- SJ_CTHL_AutoStep = 5;
- AddToTal(SJ_CTHL_TOTAL_ADDR);
- CalProSP(SJ_CTHL_SPEED_ADDR);
- if(SJ_CTHL_cZipCnt<3)
- SJ_CTHL_cZipCnt++;
- if(bSingOneFlg)
- {
- SJ_CTHL_AutoStep = 0;
- bRunning = 0;
- bSingOneFlg = 0;
- }
- }
- break;
-
- }
-
- }
- //退下模后启动送拉头
- switch(SJ_CTHL_AutoStep1)
- {
- case 1:
- /*
- if(cStopMode==1)
- {
- cStopMode=2;
- bStop=1;
- }
- */
- if(SJ_CTHL_CTM_Origin_IN)
- {
- SJ_CTHL_ZhuangLiaoStep = 1;
- SJ_CTHL_TryCnt = 0;
- SJ_CTHL_AutoStep1 = 0;
- SJ_CTHL_MGuo_VAVLE = 0;
- SJ_CTHL_JLTou_VAVLE = 0;
- }
- else if(dwTickCount >= SJ_CTHL_AutoDelay1)
- {
- //穿头模和一次穿入回位异常
- if(!SJ_CTHL_CTM_Origin_IN)
- SJ_CTHL_SetAlarmCode(SJ_CTHL_CTM_Origin_ALARM,bRunning);
- }
- break;
-
- }
-
- }
- void SJ_CTHL_CheckStart(void)
- {
-
- #if 1
- //启动 自动启动 单一自动
- if(SJ_CTHL_START_IN_UP || bStart )
- {
- if(!bRunning)
- {
- // if(SJ_CTHL_SafeDoor_IN)
- {
- bSingOneFlg= 0;
- cStopMode=0;
- bRunning = 1;
- cWULATOUCnt=0;
- SetDirReverse(X_AXIS, 1);
- SetEn(X_AXIS, MOTOR_EN);
- SetEn(Y_AXIS, MOTOR_EN);
- SJ_CTHL_FZ_MotorStep = 40;
- SJ_CTHL_AutoStep = 1;
- bTuiLaTouOkFlg = 1;
- bZhuangLiaoOkFlg= 0;
- SJ_CTHL_MGuo_VAVLE = 1; //判断是否已经有料
- SJ_CTHL_ZhuangLiaoDelay = dwTickCount + 150;
- bCL_EN_FLG = 0;
- SJ_CTHL_cZipCnt = 0;
- SJ_CTHL_LianLengthCheck = 0;
- cStopInCnt = 0;
- SJ_CTHL_bFZ_LengthOK = 0;
-
- }
- }
- bStart = 0;
- }
-
- //停止
- if(SJ_CTHL_STOP_IN_UP || bStop)
- {
- if(cStopMode < 3)
- cStopMode++;
- if(bRunning && !bSingOneFlg)// && cStopMode==2)
- {
-
- if((cStopMode >1) || SJ_CTHL_STOP_IN_UP)
- {
- bRunning=0;
- cStopMode=0;
- cXcxcrCnt=0;
- cWULATOUCnt=0;
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- SJ_CTHL_AutoStep = 0;
- SJ_CTHL_FZ_MotorStep =0;
- //SJ_CTHL_ZhuangLiaoStep=0;
- SJ_CTHL_XKDW_VAVLE =0;
- SetAlarmCode(SJ_CTHL_ALARM_ADDR,0);
- }
- else
- bSingOneFlg= 1;
- }
- else
- {
- bRunning=0;
- SJ_CTHL_RunStepSave = 0;
- cXcxcrCnt=0;
- cWULATOUCnt=0;
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- SetEn(X_AXIS, MOTOR_DISEN);
- SetEn(Y_AXIS, MOTOR_DISEN);
- // if(cStopMode >2)
- SJ_CTHL_TLiao_VAVLE=0;
- SJ_CTHL_HSLiao_VAVLE =0;
- SJ_CTHL_JLTou_VAVLE =0;
- SJ_CTHL_XKDW_VAVLE =0;
- SJ_CTHL_DXK_VAVLE =0;
- SJ_CTHL_CTXM_VAVLE =0;
- SJ_CTHL_FKGD_VAVLE =0;
- SJ_CTHL_YADAI_VAVLE = 0;
- SJ_CTHL_FWCX_VAVLE = 0;
- SJ_CTHL_BAIBI_VAVLE = 0; //摆臂
- SJ_CTHL_LYLun_VAVLE = 0; //
- SJ_CTHL_RYLun_VAVLE = 0; //Y16插销压轮
- SJ_CTHL_HELT_VAVLE = 0;
-
-
- SJ_CTHL_MGuo_VAVLE =0;
-
- SJ_CTHL_AutoStep = 0;
- SJ_CTHL_ZhuangLiaoStep=0;
- SJ_CTHL_TLiaoStep=0;
- SJ_CTHL_TD_MotorStep=0;
-
-
- SJ_CTHL_SongLiaoStep =0;
- SJ_CTHL_FZ_MotorStep =0;
- SJ_CTHL_AutoStep1 =0;
- SJ_CTHL_bFKCXTDDW_Step=0;
- SJ_CTHL_bCXCR_Step=0;
- SJ_CTHL_bCLFirst_Step=0;
- SetAlarmCode(SJ_CTHL_ALARM_ADDR,0);
- SetPos(X_AXIS, 0);
- SJ_CTHL_TLiaoStep = 0;
- SJ_CTHL_AutoStep = 0;
- }
- bStop=0;
- }
-
- if(bAlarmStop)
- {
- bRunning = 0;
- bAlarmStop=0;
- cStopMode=0;
- cXcxcrCnt=0;
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- SJ_CTHL_AutoStep = 0;
- SJ_CTHL_FZ_MotorStep =0;
- SJ_CTHL_SongLiaoStep =0;
- SJ_CTHL_ZhuangLiaoStep=0;
- // SJ_CTHL_XKTD_VAVLE = 0;
- // SJ_CTHL_XKDW_VAVLE =0;
-
-
- }
- #endif
- }
- //拖带电机控制动作 X轴
- void SJ_CTHL_TD_Motor(void) //
- {
- #if 1
- switch(SJ_CTHL_TD_MotorStep)
- {
- //1步开始,纯定位, 没有穿入
- case 1:
- if(dwTickCount >= SJ_CTHL_TD_MotorDelay) //给锁轴时间
- {
- bCheckEN_FLG = 1;
- SJ_CTHL_CheckInX02PosBuff= 0;
- SJ_CTHL_TD_MotorStep = 2;
- SetPos(X_AXIS, 0); //启动位置设为0点
- AxisContinueMoveAcc(X_AXIS,6000,DIR_P,1500,600,15,15);
- }
- break;
- case 2://检测到过链
- if(dwXRealPos > 300) //穿入2CM后退
- {
- SJ_CTHL_FWCX_VAVLE = 0; //复位插销
- }
-
- // if(dwXRealPos >= SJ_CTHL_PARAM_WLCDSHEZHI_LENTH)
- {
- // SJ_CTHL_SetAlarmCode(SJ_CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机
- }
-
- if(SJ_CTHL_FK_Check)
- {
- SJ_CTHL_FWCX_VAVLE = 0; //复位插销
- SJ_CTHL_XSavePosBuff = dwXRealPos;
- SJ_CTHL_TD_MotorStep = 3;
-
-
- AxisMovePosAccDecNotStop(X_AXIS,SJ_CTHL_PARAM_FKDW_Speed ,400,
- 3000,SJ_CTHL_PARAM_FKDW_Speed,10,10,0);
- /*
- //说明有中断
- if((dwXRealPos >= SJ_CTHL_CheckInX02PosBuff) && SJ_CTHL_CheckInX02PosBuff)
- AxisMovePosAccDec(X_AXIS,SJ_CTHL_PARAM_TD_RunSpeed,SJ_CTHL_PARAM_FKDW_Length - (dwXRealPos- SJ_CTHL_CheckInX02PosBuff),
- SJ_CTHL_PARAM_FKDW_Speed,SJ_CTHL_PARAM_FKDW_Speed,10,30,20);
- else
- AxisMovePosAccDec(X_AXIS,SJ_CTHL_PARAM_TD_RunSpeed,SJ_CTHL_PARAM_FKDW_Length,
- SJ_CTHL_PARAM_FKDW_Speed,SJ_CTHL_PARAM_FKDW_Speed,10,30,20);
- */
-
- }
- break;
- case 3:
- if(SJ_CTHL_FKDW_IN )
- {
-
- AxisMovePosAccDec(X_AXIS,GetCurSpeed(X_AXIS),SJ_CTHL_PARAM_FKDW_Length,
- SJ_CTHL_PARAM_FKDW_Speed,SJ_CTHL_PARAM_FKDW_Speed,10,30,10);
- SJ_CTHL_TD_MotorStep = 4;
- SJ_CTHL_XSavePosBuff = dwXRealPos + SJ_CTHL_PARAM_FKDW_Length;
- }
- break;
- case 4:
- if((!X_DRV) || (dwXRealPos >= SJ_CTHL_XSavePosBuff))
- {
- SJ_CTHL_TD_MotorStep = 0;
- AxisEgmStop(X_AXIS);
- }
- break;
- //穿拉头定位
- case 10:
- if(dwTickCount >= SJ_CTHL_TD_MotorDelay)
- {
- SJ_CTHL_TD_MotorStep = 11;
- SetPos(X_AXIS, 0); //启动位置设为0点
- dwXRealPos = 0;
- AxisContinueMoveAcc(X_AXIS,4000,DIR_P,2000,1200,5,5);
- SJ_CTHL_CheckInX02PosBuff= 0;
- bCheckEN_FLG = 1;
- bHeLaTouOK = 1;
- SJ_CTHL_XSavePosBuff = dwXRealPos;
- SJ_CTHL_HELT_VAVLE = 1;
- SJ_CTHL_FWCX_VAVLE = 0; //复位插销
- }
- break;
- case 11:
- if((dwXRealPos > 150) && !SJ_CTHL_CTM_Limit_IN && !SJ_CTHL_CTXM_VAVLE) //穿入2CM后退
- {
- SJ_CTHL_CheckInX02PosBuff= 0;
- SJ_CTHL_FWCX_VAVLE = 0; //复位插销
- SJ_CTHL_TD_MotorStep = 12;
- cFKCheckCnt = 0;
- SJ_CTHL_bFKCheck_Time = dwTickCount;
- /*
- if((SJ_CTHL_cZipCnt < 2))// || (SJ_CTHL_LianLengthCheck < 350))
- {
- bHeLaTouOK = 1;
- AxisContinueMoveAcc(X_AXIS,SJ_CTHL_PARAM_TD_RunSpeed,DIR_P,800,600,10,10);
- }
- else ////两段速度移动距离
- {
- */
- bHeLaTouOK = 1;
- // if(SJ_CTHL_LianLengthCheck > 1700) //太短直接下模合链
- // {
- //NotStop
- // AxisMovePosAccDecNotStop(X_AXIS,SJ_CTHL_PARAM_TD_RunSpeed*3/2 ,SJ_CTHL_LianLengthCheck * 2- SJ_CTHL_PARAM_FKDW_Length,
- // 3000,2000,10,10,0); //SJ_CTHL_PARAM_TD_RunSpeed
- // }
- // else
- AxisMovePosAccDecNotStop(X_AXIS,SJ_CTHL_PARAM_TD_RunSpeed*9/5,SJ_CTHL_LianLengthCheck +150 - dwXRealPos,
- 3000,SJ_CTHL_PARAM_TD_RunSpeed,25,20,0); //SJ_CTHL_PARAM_TD_RunSpeed
- // }
- }
- break;
- case 12://检测到过链
-
- //
-
- if(SJ_CTHL_cZipCnt)
- {
- if(((dwXRealPos + SJ_CTHL_PARAM_CL_Length) >= (SJ_CTHL_LianLengthCheck + 500)) && SJ_CTHL_LianLengthCheck)
- {
- SJ_CTHL_HELT_VAVLE = 0;
- }
- }
-
-
- if((SJ_CTHL_FKDW_IN)) //
- {
- SJ_CTHL_TD_MotorStep = 13;
- if(SJ_CTHL_cZipCnt == 0)
- SJ_CTHL_HELT_VAVLE = 0;
- SJ_CTHL_XSavePosBuff = dwXRealPos;
- SJ_CTHL_bFKCheck_Time = dwTickCount + 5;
- SJ_CTHL_XSavePosBuff1 = 0;
-
- }
- if((dwXRealPos>= (4* SJ_CTHL_PARAM_WLCDSHEZHI_LENTH)) && SJ_CTHL_PARAM_WLCDSHEZHI_LENTH)
- {
- //没链自动
- bRunning = 0;
- bAlarmStop = 1;
- SJ_CTHL_TD_MotorStep= 0;
- //
- SetAlarmCode(SJ_CTHL_ALARM_ADDR,SJ_CTHL_WLZDTJ_ALARM);
- }
- break;
- case 13:
- if(SJ_CTHL_cZipCnt)
- {
- if(((dwXRealPos + SJ_CTHL_PARAM_CL_Length) >= (SJ_CTHL_LianLengthCheck + 500)) && SJ_CTHL_LianLengthCheck)
- {
- SJ_CTHL_HELT_VAVLE = 0;
- }
- }
- if((!SJ_CTHL_FKDW_IN) && (dwXRealPos >= (SJ_CTHL_XSavePosBuff + 500)))
- {
- SJ_CTHL_TD_MotorStep = 14;
- }
- break;
- case 14:
- if(SJ_CTHL_cZipCnt)
- {
- if(((dwXRealPos + SJ_CTHL_PARAM_CL_Length) >= (SJ_CTHL_LianLengthCheck + 500)) && SJ_CTHL_LianLengthCheck)
- {
- SJ_CTHL_HELT_VAVLE = 0;
- }
- }
- //说明下一条没有方块
- if((dwXRealPos >= (SJ_CTHL_XSavePosBuff + SJ_CTHL_LianLengthCheck + 100)) && (SJ_CTHL_LianLengthCheck >1700))
- {
- SJ_CTHL_TD_MotorStep = 12;
- }
-
- if((SJ_CTHL_FK_Check)) //
- {
-
- user_datas[123] = GetCurSpeed(X_AXIS);
- user_datas[124] = dwXRealPos;
- SJ_CTHL_TD_MotorStep = 15;
- AxisMovePosAccDecNotStop(X_AXIS,2500 ,300,3000,SJ_CTHL_PARAM_FKDW_Speed,10,10,0);
- }
- break;
- case 15:
- if(SJ_CTHL_FKDW_IN)
- {
- user_datas[125] = GetCurSpeed(X_AXIS);
- user_datas[126] = dwXRealPos;
-
- AxisMovePosAccDec(X_AXIS,SJ_CTHL_PARAM_FKDW_Speed,SJ_CTHL_PARAM_FKDW_Length,
- SJ_CTHL_PARAM_FKDW_Speed,SJ_CTHL_PARAM_FKDW_Speed,2,5,0);
- SJ_CTHL_TD_MotorStep = 16;
- SJ_CTHL_bFZ_LengthOK = 1;
- SJ_CTHL_LianLengthCheck = dwXRealPos - SJ_CTHL_XSavePosBuff + SJ_CTHL_PARAM_FKDW_Length;
- SJ_CTHL_XSavePosBuff = dwXRealPos + SJ_CTHL_PARAM_FKDW_Length;
- }
- break;
- case 16:
- if((!X_DRV) || (dwXRealPos >= SJ_CTHL_XSavePosBuff))
- {
- SJ_CTHL_TD_MotorStep = 0;
- AxisEgmStop(X_AXIS);
- }
- break;
-
-
- }
- #endif
- }
- //穿入电机动作,Y轴
- void SJ_CTHL_FZ_Motor(void) //
- {
- #if 1
- //1步开始, 穿链反转长度开始
- //20步开始 插销定位开始
- switch(SJ_CTHL_FZ_MotorStep)
- {
- //穿入长度位置
- case 1:
-
- if(dwTickCount >= SJ_CTHL_FZ_MotorDelay)
- {
- SetPos(Y_AXIS, 0); //启动位置设为0点
- SetPos(X_AXIS, 0); //启动位置设为0点
- if(SJ_CTHL_bFZ_LengthOK) //确定当条拉链长度已经明确NotStop
- {
- // AxisMovePosAccDec(Y_AXIS,SJ_CTHL_PARAM_FZ_Speed,(SJ_CTHL_LianLengthCheck-700),800,600,6,6,50);
- // AxisContinueMoveAcc(Y_AXIS,(SJ_CTHL_PARAM_FZ_Speed + SJ_CTHL_PARAM_FZ_Speed/30),DIR_P,2000,1000,10,10);
- // AxisContinueMoveAcc(X_AXIS,SJ_CTHL_PARAM_FZ_Speed,DIR_N,2000,1000,10,10); //必须要保证拖带轮的线速度比后退轮的线速度大
- SJ_CTHL_LENTH = SJ_CTHL_LianLengthCheck - 400;
- AxisMovePosAccDecNotStop(Y_AXIS,(SJ_CTHL_PARAM_FZ_Speed + SJ_CTHL_PARAM_FZ_Speed/20),SJ_CTHL_LENTH,
- 2000,1050,10,15,00);
- SetDirReverse(X_AXIS, 0);
- AxisMovePosAccDecNotStop(X_AXIS,SJ_CTHL_PARAM_FZ_Speed,SJ_CTHL_LENTH - 150,
- 2000,850,10,15,00);
- }
- else //在未知长度情况下
- {
- AxisContinueMoveAcc(Y_AXIS,1300+30,DIR_P,600,600,10,10);
- AxisContinueMoveAcc(X_AXIS,1300,DIR_N,600,600,10,10); //必须要保证拖带轮的线速度比后退轮的线速度大
-
- }
- SJ_CTHL_FZ_MotorStep = 2;
- user_datas[121] = 0;
- }
- break;
- case 2:
- if(dwYRealPos > 150)
- SJ_CTHL_DXK_VAVLE = 0; //Y01顶斜口电磁阀
- if(SJ_CTHL_bFZ_LengthOK)
- {
- if((dwYRealPos + 800) > SJ_CTHL_LianLengthCheck)
- {
-
- // AxisEgmStop(X_AXIS);
- // AxisEgmStop(Y_AXIS);
- SJ_CTHL_FZ_MotorStep = 3;
- // AxisContinueMoveAcc(Y_AXIS,830,DIR_P,600,600,10,10);
- // AxisContinueMoveAcc(X_AXIS,800,DIR_N,600,600,10,10); //必须要保证拖带轮的线速度比后退轮的线速度大
- }
- }
- if((dwYRealPos > 800) && SJ_CTHL_GQ_IN)
- {
- AxisEgmStop(Y_AXIS);
- SJ_CTHL_FZ_MotorStep = 0;
- SJ_CTHL_LianLengthCheck = dwYRealPos;
- AxisEgmStop(X_AXIS);
-
- }
- // }
- /*
- // else
- {
- // if((dwYRealPos > 700) && !SJ_CTHL_XKTDW && SJ_CTHL_GQ_IN)
- {
- AxisEgmStop(Y_AXIS);
- SJ_CTHL_LianLengthCheck = dwYRealPos;
- SJ_CTHL_FZ_MotorStep = 0;
- AxisEgmStop(X_AXIS);
- // if(dwXRealPos > dwYRealPos)
- {
- // AxisMovePosAccDec(X_AXIS,3000,-(40),600,600,10,10,30);
- // AxisMovePosAccDec(Y_AXIS,1000,(20),600,600,10,10,0);
- }
-
- }
- }
- */
- break;
- case 3:
- if(SJ_CTHL_GQ_IN)
- {
-
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- SJ_CTHL_FZ_MotorStep = 0;
- SJ_CTHL_LianLengthCheck = dwYRealPos;
-
- SetDirReverse(X_AXIS, 1);
- user_datas[122] = dwXRealPos;
- user_datas[123] = dwYRealPos;//SJ_CTHL_PARAM_TD_RunSpeed/2
- }
- break;
- //夹带插销到穿入位置
- case 20: //此处最好算好定位长度相减
- SJ_CTHL_FZ_MotorStep = 21;
- SetPos(Y_AXIS, 0); //启动位置设为0点
- break;
- case 21:
- if(SJ_CTHL_PARAM_CXDW_Length)
- {
- AxisMovePosAccDec(Y_AXIS,3080,SJ_CTHL_PARAM_CXDW_Length,800,800,6,6,0);
- if(SJ_CTHL_PARAM_CXDW_Length> 20)
- AxisMovePosAccDec(X_AXIS,3000,-(SJ_CTHL_PARAM_CXDW_Length- 20),800,800,6,6,0);
- }
- SJ_CTHL_FZ_MotorStep = 22;
- break;
- case 22:
- if(!Y_DRV || (dwYRealPos >= SJ_CTHL_PARAM_CXDW_Length)) //插销定位已经结束
- {
- AxisEgmStop(Y_AXIS);
- AxisEgmStop(X_AXIS);
- SJ_CTHL_FZ_MotorStep = 0;
- }
- break;
- }
- #endif
- }
- //装拉头动作
- void SJ_CTHL_ZLT_Step(void)
- {
- switch(SJ_CTHL_ZhuangLiaoStep)
- {
- case 1:
-
- if(bZhuangLiaoOkFlg)
- {
- SJ_CTHL_ZhuangLiaoStep = 0;
- }
- else if(dwTickCount >= SJ_CTHL_ZhuangLiaoDelay)
- {
-
- if(((SJ_CTHL_MGuo_VAVLE && SJ_CTHL_LTou_Check) || (!SJ_CTHL_MGuo_VAVLE)) && !SJ_CTHL_JLiao_Limit_IN )//说明没有拉头
- {
- //穿头模和一次穿入必须在原位,接料不能有输出
- if(SJ_CTHL_CTM_Origin_IN && !SJ_CTHL_CTXM_VAVLE)
- {
- SJ_CTHL_JLTou_VAVLE = 0;
- SJ_CTHL_MGuo_VAVLE = 0;
- if(SJ_CTHL_TLiaoStep == 0)
- {
- SJ_CTHL_TLiaoStep = 1;
- SJ_CTHL_ZhuangLiaoStep = 2;
- }
- }
-
- else if(dwTickCount >= SJ_CTHL_ZhuangLiaoDelay + 500) //警告横送拉必须保证穿头模和一次穿入在原位,接料不能有输出
- {
- SJ_CTHL_SetAlarmCode(SJ_CTHL_CTM_Origin_ALARM,bRunning);
-
- }
- }
- else //已经有拉头直接跳到结束
- {
- SJ_CTHL_ZhuangLiaoDelay = dwTickCount + 40;
- SJ_CTHL_ZhuangLiaoStep = 9;
- }
- }
- break;
- case 2:
- if(SJ_CTHL_TLiaoStep == 0) //推料已经完成
- {
- SJ_CTHL_ZhuangLiaoDelay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME;
- SJ_CTHL_JLTou_VAVLE = 0;
- SJ_CTHL_MGuo_VAVLE = 0;
- if(!SJ_CTHL_JLiao_Limit_IN)
- {
- SJ_CTHL_ZhuangLiaoStep = 3;
- SJ_CTHL_HSLiao_VAVLE = 1;
- }
- }
- break;
- case 3:
- //横送到位
- if(SJ_CTHL_HSLiao_Limit_IN && !SJ_CTHL_HSLiao_Origin_IN)
- {
- SJ_CTHL_ZhuangLiaoStep = 4;
- // SJ_CTHL_JLTou_VAVLE = 1; //接拉头输出,不需要时间
- SJ_CTHL_ZhuangLiaoDelay = dwTickCount + 5;
- }
- else if(dwTickCount >= SJ_CTHL_ZhuangLiaoDelay)
- {
- SJ_CTHL_SetAlarmCode(SJ_CTHL_HSLiao_Limit_ALARM,bRunning);
- }
- break;
- case 4:
- if(dwTickCount >= SJ_CTHL_ZhuangLiaoDelay)
- {
- SJ_CTHL_ZhuangLiaoStep = 5;
- SJ_CTHL_JLTou_VAVLE = 1; //接拉头输出,不需要时间
- SJ_CTHL_ZhuangLiaoDelay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME;
- }
- break;
- case 5:
- if(SJ_CTHL_JLiao_Limit_IN)
- {
- SJ_CTHL_ZhuangLiaoStep = 6;
- SJ_CTHL_ZhuangLiaoDelay = dwTickCount + SJ_CTHL_PARAM_MaGou_DELAY;
- }
- else if(dwTickCount >= SJ_CTHL_ZhuangLiaoDelay) //接拉头(接料)到们信号异常
- {
- SJ_CTHL_SetAlarmCode(SJ_CTHL_JLiao_Limit_ALARM,bRunning);
- }
- break;
- case 6:
- if(dwTickCount >= SJ_CTHL_ZhuangLiaoDelay) //延时码勾
- {
- SJ_CTHL_MGuo_VAVLE = 1; //码勾输出
- SJ_CTHL_ZhuangLiaoStep = 7;
- SJ_CTHL_ZhuangLiaoDelay = dwTickCount + SJ_CTHL_PARAM_HSLiaoOff_DELAY;
- bTuiLaTouOkFlg = 0; //认为拉头没法带出
- }
- break;
- case 7:
- if(dwTickCount >= SJ_CTHL_ZhuangLiaoDelay)
- {
- SJ_CTHL_HSLiao_VAVLE = 0;
- SJ_CTHL_ZhuangLiaoStep = 8;
- SJ_CTHL_ZhuangLiaoDelay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME;
- }
- break;
- case 8:
- if(!SJ_CTHL_HSLiao_Limit_IN) //
- {
- SJ_CTHL_ZhuangLiaoStep = 9;
- SJ_CTHL_TLiaoStep = 1; //推拉头
- SJ_CTHL_ZhuangLiaoDelay = dwTickCount + SJ_CTHL_PARAM_CheckLT_DELAY; //也是给下模能上升的时间
- }
- else if (SJ_CTHL_LTou_Check) //时间内信号这说明没有拉头
- {
- //重送3次
- SJ_CTHL_TryCnt++;
- SJ_CTHL_JLTou_VAVLE = 0; //接拉头关
- SJ_CTHL_MGuo_VAVLE = 0; //码勾打开
- if(SJ_CTHL_TryCnt >= 3)
- {
- SJ_CTHL_TryCnt = 0;
- bZhuangLiaoOkFlg = 0;
- SJ_CTHL_ZhuangLiaoStep = 0;
- SJ_CTHL_SetAlarmCode(SJ_CTHL_ZLT_ALARM,bRunning);
- cWULATOUCnt=1;
- }
- else
- {
- SJ_CTHL_ZhuangLiaoStep = 20; //重新送3次
- SJ_CTHL_ZhuangLiaoDelay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME;
- }
- }
- else if(dwTickCount >= SJ_CTHL_ZhuangLiaoDelay)
- {
- SJ_CTHL_SetAlarmCode(SJ_CTHL_HSLiao_Limit_ALARM,bRunning);
- if(bRunning)
- {
- SJ_CTHL_ZhuangLiaoStep = 0 ;
- }
- }
- break;
- case 9:
- if(SJ_CTHL_LTou_Check) //时间内信号这说明没有拉头
- {
- SJ_CTHL_TryCnt++;
- SJ_CTHL_JLTou_VAVLE = 0; //接拉头关
- SJ_CTHL_MGuo_VAVLE = 0; //码勾打开
- if(SJ_CTHL_TryCnt >= 3)
- {
- SJ_CTHL_TryCnt=0;
- bZhuangLiaoOkFlg = 0;
- SJ_CTHL_ZhuangLiaoStep = 0;
- SJ_CTHL_SetAlarmCode(SJ_CTHL_ZLT_ALARM,bRunning);
- cWULATOUCnt=1;
- }
- else
- {
- SJ_CTHL_ZhuangLiaoStep = 20; //重新送3次
- SJ_CTHL_ZhuangLiaoDelay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME;
- }
- }
- else if(dwTickCount >= SJ_CTHL_ZhuangLiaoDelay)
- {
- bZhuangLiaoOkFlg = 1;
- SJ_CTHL_ZhuangLiaoStep = 0;
- }
- break;
- //装拉头不成攻重新送拉头
- case 20:
- if(SJ_CTHL_HSLiao_Origin_IN)
- {
- SJ_CTHL_JLTou_VAVLE = 0; //接拉头关
- SJ_CTHL_MGuo_VAVLE = 0; //码勾打开
- SJ_CTHL_ZhuangLiaoStep = 1;
- SJ_CTHL_ZhuangLiaoDelay = dwTickCount + 10;
- }
- else if(dwTickCount >= SJ_CTHL_ZhuangLiaoDelay)
- {
- SJ_CTHL_SetAlarmCode(SJ_CTHL_HSLiao_Origin_ALARM,bRunning);
- }
- break;
- }
- }
- void SJ_CTHL_TLiao_Step(void)
- {
- switch(SJ_CTHL_TLiaoStep)
- {
- case 1:
- if(bTuiLaTouOkFlg)
- {
- SJ_CTHL_TLiaoStep = 0;
- }
- else
- {
- SJ_CTHL_TLiao_Delay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME;
- SJ_CTHL_TLiaoStep = 2;
- }
- break;
- case 2: //横送料在原位
- if(SJ_CTHL_HSLiao_Origin_IN && !SJ_CTHL_HSLiao_Limit_IN && !SJ_CTHL_HSLiao_VAVLE)
- {
- SJ_CTHL_TLiao_VAVLE = 1; //推料(推拉头)输出
- SJ_CTHL_TLiao_Delay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME;
- SJ_CTHL_TLiaoStep = 3;
- }
- else if(dwTickCount >= SJ_CTHL_TLiao_Delay)
- SJ_CTHL_SetAlarmCode(SJ_CTHL_HSLiao_Origin_ALARM,1);
- break;
- case 3: //推拉头到位
- if(SJ_CTHL_TLiao_Limit_IN)
- {
- SJ_CTHL_TLiao_Delay = dwTickCount + SJ_CTHL_PARAM_TLiao_BACKDELAY;
- SJ_CTHL_TLiaoStep = 4;
- }
- else if(dwTickCount >= SJ_CTHL_TLiao_Delay)
- SJ_CTHL_SetAlarmCode(SJ_CTHL_TLiao_Limit_ALARM,1);
- break;
- case 4:
- if(dwTickCount >= SJ_CTHL_TLiao_Delay)
- {
- SJ_CTHL_TLiao_VAVLE = 0;
- SJ_CTHL_TLiao_Delay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME;
- SJ_CTHL_TLiaoStep = 5;
- }
- break;
- case 5:
- if(!SJ_CTHL_TLiao_Limit_IN)
- {
- SJ_CTHL_TLiaoStep = 0;
- SJ_CTHL_TLiao_Delay = dwTickCount;
- bTuiLaTouOkFlg = 1;
- }
- else if(dwTickCount >= SJ_CTHL_TLiao_Delay)
- SJ_CTHL_SetAlarmCode(SJ_CTHL_TLiao_Limit_ALARM,0);
- break;
- }
-
- }
- //振动盘控制
- void SJ_CTHL_ZhenDongAction(void)
- {
-
- #if 0
- if((SJ_CTHL_bDLP || bRunning) && SJ_CTHL_ZhenDongPian_OUT && SJ_CTHL_PARAM_DALIAOPIAN)
- {
- if(CT_DLP_Time >= dwTickCount)
- {
- CT_ZDP_Delay = dwTickCount + CT_PARAM_DLP_TIME + 40;
- CT_DaLaPian_VAVLE = ~CT_DaLaPian_VAVLE;
- }
- }
- else
- CT_DaLaPian_VAVLE = 0;
- #endif
-
- #if 1
- // if(bRunning)
- // {
- // if(CT_PARAM_ZDP_MODE)
- // {
- if(SJ_CTHL_ZhenDongPian_OUT == 0)
- {
- if(!SJ_CTHL_ZDP_IN)
- {
- if(dwTickCount >= SJ_CTHL_ZDP_Delay)
- {
- SJ_CTHL_ZhenDongPian_OUT = 1;
- SJ_CTHL_ZDP_Delay = dwTickCount + SJ_CTHL_PARAM_ZDP_Stop_TIME;//SJ_CTHL_PARAM_ZDP_Stop_TIME + 50;
- }
- }
- else
- {
- SJ_CTHL_ZDP_Delay = dwTickCount + SJ_CTHL_PARAM_ZDP_Start_TIME;//SJ_CTHL_PARAM_ZDP_Start_TIME;
- }
- }
- else
- {
- if(SJ_CTHL_ZDP_IN)
- {
- if(dwTickCount >= SJ_CTHL_ZDP_Delay)
- {
- SJ_CTHL_ZhenDongPian_OUT = 0;
- SJ_CTHL_ZDP_Delay = dwTickCount + SJ_CTHL_PARAM_ZDP_Start_TIME;//SJ_CTHL_PARAM_ZDP_Start_TIME
- }
- }
- else
- SJ_CTHL_ZDP_Delay = dwTickCount + SJ_CTHL_PARAM_ZDP_Stop_TIME;//SJ_CTHL_PARAM_ZDP_Stop_TIME+ 50;
- }
- #endif
- }
- void SJ_CTHL_bFKCXTDDWStep(void)
- {
- #if 0
- switch(SJ_CTHL_bFKCXTDDW_Step)
- {
- case 1:
- SJ_CTHL_FKJD_VAVLE = 1; //方块夹带气缸输出
- SJ_CTHL_XKTD_VAVLE = 1; //斜口推带气缸输出
- SJ_CTHL_FKGD_VAVLE = 0; //方块固定关
- SJ_CTHL_bFKCXTDDW_Step = 2;
-
- break;
- case 2:
- if(!SJ_CTHL_XKTD_Origin_IN) //斜口推带原位离开
- {
- SJ_CTHL_bFKCXTDDW_Step = 3;
- SJ_CTHL_bFKCXTDDW_Delay = dwTickCount + 20;
- }
- break;
- case 3://斜口定位
- if(dwTickCount >= SJ_CTHL_bFKCXTDDW_Delay)
- {
- SJ_CTHL_XKDW_VAVLE = 1;
- SJ_CTHL_bFKCXTDDW_Step = 4;
- SJ_CTHL_bFKCXTDDW_Delay = dwTickCount + SJ_CTHL_PARAM_XKDW_TD_DELAY;
- }
- break;
- case 4:
- if(dwTickCount >= SJ_CTHL_bFKCXTDDW_Delay)
- {
- AxisContinueMoveAcc(X_AXIS,1200,DIR_P,800,600,10,10);//拖带电机
- SJ_CTHL_bFKCXTDDW_Step = 5;
- SJ_CTHL_XSavePosBuff = dwXRealPos;
- SetPos(X_AXIS, 0); //启动位置设为0点
- dwXRealPos = 0;
- }
- break;
- case 5:
- if(SJ_CTHL_XKSC_Limit_IN) //方块到位
- {
- AxisEgmStop(X_AXIS);
- SJ_CTHL_bFKCXTDDW_Step = 0;
- SJ_CTHL_XKTD_VAVLE = 0; //斜口推带气缸关闭
- SJ_CTHL_bFKCXTDDW_Delay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME;
-
- }
- else if(dwXRealPos >= (SJ_CTHL_XSavePosBuff + 800))
- {
- SJ_CTHL_SetAlarmCode(SJ_CTHL_FKCXDW_ALARM,bRunning);//方块插销定位异常警告
-
- }
- break;
-
- }
- #endif
- }
- //插销穿入动作
- void SJ_CTHL_bCXCRStep(void)
- {
- #if 0
- switch(SJ_CTHL_bCXCR_Step)
- {
- case 11:
- SJ_CTHL_XKTD_VAVLE = 1; //斜口推带气缸输出
- SJ_CTHL_FKGD_VAVLE = 0; //方块固定关
- SJ_CTHL_bCXCR_Step = 12;
- break;
- case 12:
- if(!SJ_CTHL_XKTD_Origin_IN) //斜口推带原位离开
- {
- SJ_CTHL_bCXCR_Step = 13;
- SJ_CTHL_bCXCR_Delay = dwTickCount + 20;
- }
- break;
- case 13:
- if((SJ_CTHL_CL_MotorStep == 0) && (SJ_CTHL_SF_Origin_IN) && (dwTickCount >= SJ_CTHL_bCXCR_Delay)) //插销前夹带回到原位
- {
- SJ_CTHL_bCXCR_Step = 14;
- SJ_CTHL_FKGD_VAVLE = 1; //方块固定关
- SJ_CTHL_CXQJD_VAVLE = 1; //插销前夹带
- SJ_CTHL_bCXCR_Delay = dwTickCount + SJ_CTHL_PARAM_XCQJD_XCDW_DELAY;
- }
- break;
- case 14:
- if(dwTickCount >= SJ_CTHL_bCXCR_Delay)
- {
- SJ_CTHL_bCXCR_Step = 15;
- SJ_CTHL_FKGD_VAVLE = 1; //方块固定
- //Y轴启动插销
- SJ_CTHL_CL_MotorStep = 20; //插销定位
- }
- break;
- case 15:
- if(SJ_CTHL_CL_MotorStep == 0) //插销定位完成
- {
- SJ_CTHL_bCXCR_Step = 16;
- SJ_CTHL_CXDWZ_VAVLE = 1; //插销定位针(到位)电磁阀
- SJ_CTHL_bCXCR_Delay = dwTickCount + 10; //延时插销下移
- }
- break;
- case 16:
- if(dwTickCount >= SJ_CTHL_bCXCR_Delay)
- {
- SJ_CTHL_bCXCR_Step = 17;
- SJ_CTHL_CXXY_VAVLE = 1; //Y06 插销下移电磁阀
- SJ_CTHL_bCXCR_Delay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME; //插销下移异常时间
- }
- break;
- case 17:
- if(SJ_CTHL_CXXY_Limit_IN)
- {
- // if(SJ_CTHL_START_IN_UP)
- {
- SJ_CTHL_bCXCR_Step = 18;
- if( cXcxcrCnt==0 && !X_DRV) //拖带电机反转
- {
- // SetDir(X_AXIS, DIR_N);
- AxisMovePosAccDec(X_AXIS,10000,-SJ_CTHL_PARAM_TDFZ_LENTH,800,600,6,6,0);
- }
- SJ_CTHL_CXQJD_VAVLE = 0; //插销前夹带电磁阀
- SJ_CTHL_CXSCDW_VAVLE = 0; //Y21 插销上止定位关
- SJ_CTHL_CXHJD_VAVLE = 1; //Y14 插销后夹带输出
- SJ_CTHL_bCXCR_Delay = dwTickCount + SJ_CTHL_PARAM_SCDWOff_CXDW_DELAY;
- }
- }
- else if(dwTickCount >= SJ_CTHL_bCXCR_Delay)
- {
- SJ_CTHL_SetAlarmCode(SJ_CTHL_CXXY_Limit_ALARM,bRunning);//插销下移异常
-
- }
- break;
- case 18:
- // if(START_IN_UP)
- {
- if(dwTickCount >= SJ_CTHL_bCXCR_Delay)
- {
- SJ_CTHL_bCXCR_Step = 19;
-
- SJ_CTHL_CXHDW_VAVLE = 1; //插销后定位(插销穿入)
- SJ_CTHL_bCXCR_Delay = dwTickCount + SJ_CTHL_VAVLE_ERROR_TIME +1000; //插销穿入到位异常时间
- SJ_CTHL_CL_MotorStep = 50; //合链电机回原点
- }
- }
- break;
- case 19:
- if( cXcxcrCnt > 1)
- {
- SJ_CTHL_SetAlarmCode(SJ_CTHL_CX_Limit_ALARM,bRunning); //插销穿入到位异常
- }
-
- if( SJ_CTHL_CX_Limit_IN && cXcxcrCnt<2 )
- {
- cXcxcrCnt=0;
- bZhuangLiaoOkFlg = 0;
- SJ_CTHL_bCXCR_Step = 0;
- SJ_CTHL_CXDWZ_VAVLE = 0; //插销定位针(到位)电磁阀
-
- }
- else if (dwTickCount >= SJ_CTHL_bCXCR_Delay)
- {
- cXcxcrCnt++;
-
- SJ_CTHL_CXHJD_VAVLE=0;
- SJ_CTHL_CXHDW_VAVLE=0;
- SJ_CTHL_CXDWZ_VAVLE = 0;
- SJ_CTHL_CXXY_VAVLE=0;
- SJ_CTHL_bCXCR_Step = 11;
- }
-
- break;
- }
- #endif
- }
- //手动动作
- void SJ_CTHL_ManualAction(void)
- {
- if(bRunning) // 运行灯输出
- {
- SJ_CTHL_Run_State=1;
- SJ_CTHL_Stop_State=0;
- }
- else
- {
- SJ_CTHL_Stop_State=1;
- SJ_CTHL_Run_State=0;
- }
-
-
-
- if(bRunning == 0)
- {
- if(bClearTotal) //切断计数清零
- {
- bClearTotal = 0;
- ClrcToTal(QDCT_TOTAL_ADDR);
- }
- if(SJ_CTHL_bDXK) //顶斜口
- {
- SJ_CTHL_bDXK = 0;
- if(SJ_CTHL_DXK_VAVLE)
- SJ_CTHL_DXK_VAVLE = 0;
- else
- SJ_CTHL_DXK_VAVLE = 1;
-
- }
- if(SJ_CTHL_bXKDW) //斜口定位
- {
- SJ_CTHL_bXKDW = 0;
- SJ_CTHL_XKDW_VAVLE = ~SJ_CTHL_XKDW_VAVLE;
- }
- if(SJ_CTHL_bFWCX) //复位插销
- {
- SJ_CTHL_bFWCX = 0;
- SJ_CTHL_FWCX_VAVLE = ~SJ_CTHL_FWCX_VAVLE;
- }
-
- if(SJ_CTHL_bBB) //摆臂
- {
- SJ_CTHL_bBB = 0;
- SJ_CTHL_BAIBI_VAVLE = ~SJ_CTHL_BAIBI_VAVLE;
- }
- //退插销
- //夹带
-
- if(SJ_CTHL_bTLiao ) //手动推料(推拉头)
- {
- SJ_CTHL_bTLiao = 0;
- if(SJ_CTHL_TLiao_VAVLE)
- SJ_CTHL_TLiao_VAVLE = 0;//SJ_CTHL_TLiao_VAVLE;
- else if(!SJ_CTHL_HSLiao_VAVLE && SJ_CTHL_HSLiao_Origin_IN)
- {
- SJ_CTHL_TLiao_VAVLE = 1;
- }
- else//条件警告
- {
- SJ_CTHL_SetAlarmCode(SJ_CTHL_HENSONGLIAO_ALARM,0); // 横送料不能输出
- }
- }
- if(SJ_CTHL_bHSL) //手动横送料
- {
- SJ_CTHL_bHSL= 0;
- if(SJ_CTHL_HSLiao_VAVLE)
- {
- SJ_CTHL_HSLiao_VAVLE = 0;//~SJ_CTHL_HSLiao_VAVLE;
- }
- else if(!SJ_CTHL_TLiao_VAVLE && !SJ_CTHL_TLiao_Limit_IN && SJ_CTHL_CTM_Origin_IN && !SJ_CTHL_JLiao_Limit_IN &&
- !SJ_CTHL_JLTou_VAVLE && !SJ_CTHL_CTXM_VAVLE && (!SJ_CTHL_bMG || (SJ_CTHL_bMG && SJ_CTHL_LTou_Check)))
- {
- SJ_CTHL_HSLiao_VAVLE = 1;
- }
- else //条件警告 最后一个括号条件为已经有拉头
- {
- SJ_CTHL_SetAlarmCode(SJ_CTHL_YCCT_ALARM,0);
- }
- }
- if(SJ_CTHL_bJLT) //接拉头
- {
- SJ_CTHL_bJLT = 0;
- if(SJ_CTHL_JLTou_VAVLE)
- SJ_CTHL_JLTou_VAVLE = 0;
- else if( !SJ_CTHL_CTXM_VAVLE && SJ_CTHL_CTM_Origin_IN)
- {
- SJ_CTHL_MGuo_VAVLE = 0;
- SJ_CTHL_JLTou_VAVLE = 1;
- }
- else//条件警告
- {
- SJ_CTHL_SetAlarmCode(SJ_CTHL_YCXM_Origin_ALARM,0);
- }
- }
- if(SJ_CTHL_bMG) //码勾
- {
- SJ_CTHL_bMG = 0;
- SJ_CTHL_MGuo_VAVLE = !SJ_CTHL_MGuo_VAVLE;
- }
-
-
- if(SJ_CTHL_bCTXM) //穿头下模
- {
- SJ_CTHL_bCTXM = 0;
- if(SJ_CTHL_CTXM_VAVLE)
- {
- SJ_CTHL_CTXM_VAVLE = 0;
- SJ_CTHL_JLTou_VAVLE = 0;
- }
- else if(!SJ_CTHL_HSLiao_VAVLE && SJ_CTHL_HSLiao_Origin_IN)
- {
- SJ_CTHL_MGuo_VAVLE = 1;
- SJ_CTHL_CTXM_VAVLE = 1;
- }
- }
- if(SJ_CTHL_bLYLun) //左压轮
- {
- SJ_CTHL_bLYLun = 0;
- SJ_CTHL_LYLun_VAVLE = ~SJ_CTHL_LYLun_VAVLE;
- }
- if(SJ_CTHL_bRYLun) //右压轮
- {
- SJ_CTHL_bRYLun= 0;
- SJ_CTHL_RYLun_VAVLE = ~SJ_CTHL_RYLun_VAVLE;
- }
-
-
- if(SJ_CTHL_bFKGD) //方块固定
- {
- SJ_CTHL_bFKGD = 0;
- SJ_CTHL_FKGD_VAVLE = ~SJ_CTHL_FKGD_VAVLE;
- }
- //合拉头
- if(SJ_CTHL_bHeLT)
- {
- SJ_CTHL_bHeLT = 0;
- SJ_CTHL_HELT_VAVLE = ~SJ_CTHL_HELT_VAVLE;
- }
- //压带
- if(SJ_CTHL_bYD)
- {
- SJ_CTHL_bYD = 0;
- SJ_CTHL_YADAI_VAVLE =~SJ_CTHL_YADAI_VAVLE;
- }
-
-
- //自动装料
- if(SJ_CTHL_bAutoZhuangLiao)
- {
- SJ_CTHL_bAutoZhuangLiao = 0;
- if(SJ_CTHL_ZhuangLiaoStep == 0)
- {
- bTuiLaTouOkFlg = 1;
- bZhuangLiaoOkFlg = 0;
- SJ_CTHL_ZhuangLiaoStep = 1;
- SJ_CTHL_MGuo_VAVLE = 1; //判断是否已经有料
- SJ_CTHL_ZhuangLiaoDelay = dwTickCount + 150;
- }
- }
- //测试电机
- if(SJ_CTHL_bCLMotor_N)
- {
- SetEn(Y_AXIS, MOTOR_EN);
-
- if(!Y_DRV)
- // Y轴 运行速度 启动速度 加速度 减速度
- AxisContinueMoveAcc(Y_AXIS,2000,DIR_N,1000,1000,15,15);
- }
-
- if(SJ_CTHL_bCLMotor_P) //后退限位已经取消
- {
- SetEn(Y_AXIS, MOTOR_EN);
- if(!Y_DRV)
- // Y轴 运行速度 启动速度 加速度 减速度
- AxisContinueMoveAcc(Y_AXIS,2000,DIR_P,1000,1000,15,15);
- }
-
- if(!SJ_CTHL_bCLMotor_P && !SJ_CTHL_bCLMotor_N && (SJ_CTHL_FZ_MotorStep == 0))
- {
- AxisEgmStop(Y_AXIS);//AxisDecStop(Y_AXIS);
- }
- //合链电机返回原点
- if(SJ_CTHL_bCLMotor_O)
- {
- SJ_CTHL_bCLMotor_O = 0;
- if(SJ_CTHL_FZ_MotorStep == 0)
- SJ_CTHL_FZ_MotorStep = 40;
- }
- //拖带方块定位 ,因这步为前拖带定位, 很多电磁阀要关掉
- if(SJ_CTHL_bFKTDDW)
- {
- SJ_CTHL_bFKTDDW = 0;
- if(SJ_CTHL_TD_MotorStep == 0)
- {
- SJ_CTHL_XKDW_VAVLE = 0; //斜口定位
-
- if((GetEn(X_AXIS) == MOTOR_DISEN))
- {
- SetEn(X_AXIS, MOTOR_EN);
- SJ_CTHL_TD_MotorDelay = dwTickCount + 200;
- }
- else
- SJ_CTHL_TD_MotorDelay = dwTickCount + 50;
- SJ_CTHL_TD_MotorStep = 1;
- }
- }
- //拖带方块插销定位(斜口推带,斜口定位,顶斜口)
- if(SJ_CTHL_bFKCXTDDW)
- {
- SJ_CTHL_bFKCXTDDW = 0;
- if((SJ_CTHL_bFKCXTDDW_Step == 0) && (SJ_CTHL_FZ_MotorStep == 0) &&
- !SJ_CTHL_CTXM_VAVLE)
- {
- SJ_CTHL_bFKCXTDDW_Step = 1;
- SetEn(X_AXIS, MOTOR_EN);
-
- }
- }
- //插销穿入(包含电机前定位,插销下移,后夹,后定位)
- if(SJ_CTHL_bCXCR)
- {
- SJ_CTHL_bCXCR = 0;
- //拉头模在上面到们并且码勾输出,判断有拉头
- if(SJ_CTHL_CTXM_VAVLE && SJ_CTHL_MGuo_VAVLE &&
- SJ_CTHL_CTM_Limit_IN && !SJ_CTHL_LTou_Check)
- {
- if(SJ_CTHL_FZ_MotorStep == 0)
- {
- SJ_CTHL_bCXCR_Step = 11;
- SetEn(Y_AXIS, MOTOR_EN);
- }
- }
- else //警告不能自动穿插销
- {
- SJ_CTHL_SetAlarmCode(SJ_CTHL_WFZIDONGCX_ALARM,0);
- }
- }
- //电机合链动作
- if(SJ_CTHL_bMotorHL)
- {
- SJ_CTHL_bMotorHL = 0;
- }
- //一次穿链动作
- if(SJ_CTHL_bCL_First)
- {
- SJ_CTHL_bCL_First = 0;
- //拉头模在上面到们并且码勾输出,判断有拉头
- if(SJ_CTHL_CTXM_VAVLE && SJ_CTHL_MGuo_VAVLE &&
- SJ_CTHL_CTM_Limit_IN && !SJ_CTHL_LTou_Check && (SJ_CTHL_FZ_MotorStep == 0))
- {
- SJ_CTHL_bCLFirst_Step = 19;
- }
- }
- /*
- SJ_CTHL_LianLengthCheck = 6989;
- if(SJ_CTHL_bTiaoShiStep)
- {
- SJ_CTHL_bTiaoShiStep = 0;
- SetPos(X_AXIS,0);
- SetPos(Y_AXIS,0);
- dwXRealPos = 0;
- dwYRealPos = 0;
- AxisMovePosAccDec(Y_AXIS,SJ_CTHL_PARAM_FZ_Speed,-SJ_CTHL_LianLengthCheck,
- 2000,1000,10,15,200);
- AxisMovePosAccDec(X_AXIS,SJ_CTHL_PARAM_FZ_Speed,(SJ_CTHL_LianLengthCheck),
- 2000,1000,10,15,200);
- }
-
- if(SJ_CTHL_bTiaoShiMode)
- {
- SJ_CTHL_bTiaoShiMode = 0;
- SetPos(X_AXIS,0);
- SetPos(Y_AXIS,0);
- dwXRealPos = 0;
- dwYRealPos = 0;
- AxisMovePosAccDec(Y_AXIS,SJ_CTHL_PARAM_FZ_Speed,SJ_CTHL_LianLengthCheck,
- 2000,1000,10,15,200);
- AxisMovePosAccDec(X_AXIS,SJ_CTHL_PARAM_FZ_Speed,-(SJ_CTHL_LianLengthCheck),
- 2000,1000,10,15,200);
- }
- */
- }
- }
- #endif
|