123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872187318741875187618771878187918801881188218831884188518861887188818891890189118921893189418951896189718981899190019011902190319041905190619071908190919101911191219131914191519161917191819191920192119221923192419251926192719281929193019311932193319341935193619371938193919401941194219431944194519461947194819491950195119521953195419551956195719581959196019611962196319641965196619671968196919701971197219731974197519761977197819791980198119821983198419851986198719881989199019911992199319941995199619971998199920002001200220032004200520062007200820092010201120122013201420152016201720182019202020212022202320242025202620272028202920302031203220332034203520362037203820392040204120422043204420452046204720482049205020512052205320542055205620572058205920602061206220632064206520662067206820692070207120722073207420752076207720782079208020812082208320842085208620872088208920902091209220932094209520962097209820992100210121022103210421052106210721082109211021112112211321142115211621172118211921202121212221232124212521262127212821292130213121322133213421352136213721382139214021412142214321442145214621472148214921502151215221532154215521562157215821592160216121622163216421652166216721682169217021712172217321742175217621772178217921802181218221832184218521862187218821892190219121922193219421952196219721982199220022012202220322042205220622072208220922102211221222132214221522162217221822192220222122222223222422252226222722282229223022312232223322342235223622372238223922402241224222432244224522462247224822492250225122522253225422552256225722582259226022612262226322642265226622672268226922702271227222732274227522762277227822792280228122822283228422852286228722882289229022912292229322942295229622972298229923002301230223032304230523062307230823092310231123122313231423152316231723182319232023212322232323242325232623272328232923302331233223332334233523362337233823392340234123422343234423452346234723482349235023512352235323542355235623572358235923602361236223632364236523662367236823692370237123722373237423752376237723782379238023812382238323842385238623872388238923902391239223932394239523962397239823992400240124022403240424052406240724082409241024112412241324142415241624172418241924202421242224232424242524262427242824292430243124322433243424352436243724382439244024412442244324442445244624472448244924502451245224532454245524562457245824592460246124622463246424652466246724682469247024712472247324742475247624772478247924802481248224832484248524862487248824892490249124922493249424952496249724982499250025012502250325042505250625072508250925102511251225132514251525162517251825192520252125222523252425252526252725282529253025312532253325342535253625372538253925402541254225432544254525462547254825492550255125522553255425552556255725582559256025612562256325642565256625672568256925702571257225732574257525762577257825792580258125822583258425852586258725882589259025912592259325942595259625972598259926002601260226032604260526062607260826092610261126122613261426152616261726182619262026212622262326242625262626272628262926302631263226332634263526362637263826392640264126422643264426452646264726482649265026512652265326542655265626572658265926602661266226632664266526662667266826692670267126722673267426752676267726782679268026812682268326842685268626872688268926902691269226932694269526962697269826992700270127022703270427052706270727082709271027112712271327142715271627172718271927202721272227232724272527262727272827292730273127322733273427352736273727382739274027412742274327442745274627472748274927502751275227532754275527562757275827592760276127622763276427652766276727682769277027712772277327742775277627772778 |
- #include "global.h"
- #if LONG_YI_MACHINE == 1
- void LY_CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag);
- void LY_CTHL_ManualAction(void);
- void LY_CTHL_AutoStepAction(void);
- void LY_CTHL_TableAction(void);
- void LY_CTHL_TD_Motor(void);
- void LY_CTHL_CL_Motor(void);
- void LY_CTHL_CheckStart(void);
- void LY_CTHL_TLiao_Step(void); //推料(推拉头)
- void LY_CTHL_AlarmProtect(void);
- void LY_CTHL_ZLT_Step(void);
- void LY_CTHL_ZhenDongAction(void);
- void LY_CTHL_bFKCXTDDWStep(void);
- void LY_CTHL_bCXCRStep(void);
- void LY_CTHL_bCLFirstStep(void);
- void ExtiAcitionX02(void)
- {
- if(bCheckEN_FLG)
- {
- bCheckEN_FLG = 0;
- LY_CTHL_CheckInX02PosBuff = GetPos(X_AXIS);
- }
- }
- //故障报警
- void LY_CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag)
- {
- SetAlarmCode(LY_CTHL_ALARM_ADDR,alarm_code);
- // bAlarmStop = 0;
- // if(cStopFlag)
- if(!cWULATOUCnt && !cXiaoChaCNT && !cCHULIANBAOJIN)
- {
- bAlarmStop = 1;
- }
- else
- {
- cWULATOUCnt=0;
- }
-
-
-
- }
- //初始化动作
- void LY_CTHL_InitAction(void)
- {
- float buff_pulse,buff_dist;
-
- buff_pulse = LY_CTHL_PARAM_CYCLE_PULSE;
- buff_dist = LY_CTHL_PARAM_CYCLE_LENGTH;
-
- XGearRatio = buff_pulse/buff_dist;
-
- buff_pulse = 2000;//LY_CTHL_PARAM_CYCLE_PULSE;
- buff_dist = 750;//LY_CTHL_PARAM_CYCLE_LENGTH;
- YGearRatio = buff_pulse/buff_dist;
-
-
-
- bYFirstDingWeiFlg = 1;
- user_datas[121] = 0;
- user_datas[122] = 0;
- user_datas[123] = 0;
-
- SetEnReverse(X_AXIS, 1);
- SetEnReverse(Y_AXIS, 1);
- SetEnReverse(Z_AXIS, 1);
-
- SetDirReverse(X_AXIS, 0);
- SetDirReverse(Y_AXIS, 0);
- SetDirReverse(Z_AXIS, 0);
-
- SetEn(X_AXIS, MOTOR_DISEN);
- SetEn(Y_AXIS, MOTOR_DISEN);
- SetEn(Z_AXIS, MOTOR_DISEN);
-
- SetAlarmCode(LY_CTHL_ALARM_ADDR,0);
- // QDCT_SZ_OUT = QDCT_MOTOR_DISEN;
- }
- void LY_CTHL_Action(void)
- {
- user_datas[121] = LY_CTHL_LianLengSave;
- // user_datas[122] =axis_x->cur_speed;
- // user_datas[123] = LY_CTHL_bCXCR_Step;
- dwXRealPos = GetPos(X_AXIS);
- dwYRealPos = GetPos(Y_AXIS);
- dwZRealPos = GetPos(Z_AXIS);
- user_datas[124] = dwXRealPos;
- user_datas[125] = dwYRealPos;
-
- user_datas[126] = LY_CTHL_CL_MotorStep;
- user_datas[127] = LY_CTHL_TD_MotorStep;
- user_datas[128] = LY_CTHL_AutoStep;
-
- LY_CTHL_ZhenDongAction();
- LY_CTHL_AlarmProtect();
- LY_CTHL_CheckStart();
- LY_CTHL_ZLT_Step();
- LY_CTHL_TLiao_Step();
- LY_CTHL_TD_Motor();
- LY_CTHL_CL_Motor();
- LY_CTHL_bFKCXTDDWStep();
- LY_CTHL_bCXCRStep();
- LY_CTHL_bCLFirstStep();
- LY_CTHL_ManualAction();
- LY_CTHL_AutoStepAction();
- }
- void LY_CTHL_AlarmProtect(void)
- {
-
-
- }
- //自动动作
- void LY_CTHL_AutoStepAction(void)
- {
-
- if(bRunning)
- {
- switch(LY_CTHL_AutoStep)
- {
- case 1:
- if(LY_CTHL_SF_Origin_IN || cBanAuTo)
- {
- /* LY_CTHL_AutoStep = 10; //测试拖带定位
- LY_CTHL_AutoDelay = dwTickCount + 1000;
- break; */
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_FKJD_VAVLE = 1; //方块夹带气缸输出
- LY_CTHL_XKTD_VAVLE = 1; //斜口推带气缸输出
- LY_CTHL_FKGD_VAVLE = 0; //方块固定关
- // LY_CTHL_XKDW_VAVLE = 1;
- LY_CTHL_AutoStep = 2;
- // LY_CTHL_AutoDelay = dwTickCount + 10;
- if(LY_CTHL_ZhuangLiaoStep == 0)
- {
- LY_CTHL_ZhuangLiaoStep = 1;
- LY_CTHL_TryCnt = 0;
- }
- }
- }
- break;
- case 2:
- if(!LY_CTHL_XKTD_Origin_IN)// && (dwTickCount >= LY_CTHL_AutoDelay)) //斜口推带原位离开
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_AutoStep = 3;
- LY_CTHL_XKDW_VAVLE = 1;
- LY_CTHL_AutoDelay = dwTickCount + 20;
- }
- }
- break;
- case 3://斜口定位
- if(dwTickCount >= LY_CTHL_AutoDelay)
- {
- LY_CTHL_AutoStep = 4;
- // LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_PARAM_XKDW_TD_DELAY;
- }
- break;
- case 4:
- if(!cBanAuTo)
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- AxisMovePosAccDecNotStop(X_AXIS,1600,150,600,1000,5,3,00);
- LY_CTHL_AutoStep = 5;
- LY_CTHL_XSavePosBuff = dwXRealPos;
- // SetPos(X_AXIS, 0); //启动位置设为0点
- if(cZipCnt < 1)
- LY_CTHL_AutoDelay = dwTickCount +350;
- else
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_PARAM_FKJD_DELAY;
- }
- }
- else
- {
- LY_CTHL_AutoStep = 5;
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_PARAM_FKJD_DELAY;
- LY_CTHL_XSavePosBuff = dwXRealPos;
- }
-
- break;
- case 5:
- if(dwTickCount >= LY_CTHL_AutoDelay)
- {
- LY_CTHL_FKJD_VAVLE = 1; //方块夹带气缸输出
-
- }
- if(bCL_EN_FLG && (dwXRealPos >= 230) && LY_CTHL_XKSC_Limit_IN)
- {
- LY_CTHL_HLJJ_VAVLE = 1; //Y20 合链夹具电磁阀输出后,电机穿入
- }
- if(LY_CTHL_XKSC_Limit_IN) //方块上止到位
- {
- AxisEgmStop(X_AXIS);
- // AxisMovePosAccDec(X_AXIS,1200,10,1200,1200,6,6,0);
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_QDXK_VAVLE = 1;
- LY_CTHL_FKJD_VAVLE=0;
-
- LY_CTHL_AutoStep = 6;
- //
-
- LY_CTHL_bLTCHECK_Delay = dwTickCount + 20;
- //电机穿入可以启动
- bCL_OK_FLG = 1; //不用穿合链的话认为已经完成
- if(bCL_EN_FLG)
- {
- bCL_OK_FLG = 0;
- LY_CTHL_HLJJ_VAVLE = 1; //Y20 合链夹具电磁阀输出后,电机穿入
- LY_CTHL_CL_MotorStep = 1;
- LY_CTHL_CL_MotorDelay = dwTickCount + LY_CTHL_PARAM_HLJJ_CL_DELAY;
-
- LY_CTHL_AutoDelay = dwTickCount + 50 + LY_CTHL_PARAM_HLJJ_CL_DELAY;
- }
- else
- {
- LY_CTHL_XKTD_VAVLE = 0; //斜口推带气缸关闭
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME;
- }
- }
- }
- else if(dwXRealPos >= (LY_CTHL_XSavePosBuff + 3000))
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_FKCXDW_ALARM,bRunning); //方块插销定位异常警告
-
- }
- break;
- case 6:
- if(((dwYRealPos > 1000) || (!Y_DRV && dwYRealPos)) && LY_CTHL_XKTD_VAVLE)
- {
- LY_CTHL_XKTD_VAVLE = 0; //斜口推带气缸关闭
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME;
- }
-
- if(LY_CTHL_XKTD_Origin_IN) //斜口推带回到原位
- {
- if(bZhuangLiaoOkFlg && (LY_CTHL_ZhuangLiaoStep == 0))
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- if(dwTickCount >= LY_CTHL_bLTCHECK_Delay)
- {
- if(bSingOneFlg)
- {
- LY_CTHL_AutoStep = 0;
- bRunning = 0;
- }
- else
- {
- if(!LY_CTHL_LTou_Check)
- {
- if(LY_CTHL_PARAM_TDFZ_LENTH && !X_DRV) //拖带电机反转
- {
- AxisMovePosAccDec(X_AXIS,8000,-LY_CTHL_PARAM_DXKFZ_LENTH,800,600,6,6,0);
- if(!LY_CTHL_PARAM_NLSL_MODE)
- LY_CTHL_FKJD_VAVLE= 0; //方块夹带气缸关
-
- }
- if(LY_CTHL_PARAM_CXLY_MODE)
- LY_CTHL_CXLY_VAVLE = 1;
- LY_CTHL_CTXM_VAVLE = 1; //穿头下模上升
- LY_CTHL_AutoStep = 7;
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME;
- }
- else
- {
- cWULATOUCnt=1;
- LY_CTHL_SetAlarmCode(LY_CTHL_ZLT_ALARM,bRunning); // 没有拉头
- }
- }
- }
- }
- }
- }
- else if((dwTickCount >= LY_CTHL_AutoDelay) && !LY_CTHL_XKTD_VAVLE && !cBanAuTo)
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_XKTD_Origin_ALARM,bRunning); //斜口推带回到异常
- }
- break;
- case 7:
- if(!LY_CTHL_CTM_Origin_IN)
- {
- LY_CTHL_JLTou_VAVLE = 0; //接拉头
- }
- if(LY_CTHL_CTM_Limit_IN)
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- if(LY_CTHL_PARAM_NLSL_MODE)
- {
-
- LY_CTHL_XKTD_VAVLE=1;
- }
- LY_CTHL_JLTou_VAVLE = 0;
- LY_CTHL_AutoStep = 8;
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_PARAM_DXKYS_DELAY;
- }
-
- }
- else if(dwTickCount >= LY_CTHL_AutoDelay)
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_CTM_Limit_ALARM,bRunning); //穿头下模上升异常
- }
- break;
- case 8:
- if(!X_DRV) //延时顶斜口
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- if((dwTickCount >= LY_CTHL_AutoDelay) || (LY_CTHL_PARAM_NLSL_MODE))
- {
- if(!LY_CTHL_PARAM_NLSL_MODE)
- LY_CTHL_DXK_VAVLE=1;
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_PARAM_DXK_XKDW_Off_DELAY;
- LY_CTHL_AutoStep = 9;
- }
- }
- }
- break;
- case 9:
- if((dwTickCount >= LY_CTHL_AutoDelay) && (bCL_OK_FLG)) //顶斜口延时退斜口上止定位
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- // AxisMovePosAccDec(X_AXIS,2000,-150,1200,1200,3,6,0); //反转1.5CM
- LY_CTHL_XKDW_VAVLE = 0; //斜口上止定位关
- LY_CTHL_FKJD_VAVLE = 0; //方块夹带气缸关
- LY_CTHL_AutoStep = 10;
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_PARAM_XK_CR_DELAY;
- }
- }
- break;
- case 10:
- if(!cBanAuTo)
- {
- if((dwTickCount >= LY_CTHL_AutoDelay) && !X_DRV)
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_TD_MotorStep = 10; //启动拖带穿入
- LY_CTHL_AutoStep = 11;
- }
- }
- }
- else
- {
- LY_CTHL_AutoStep = 801;
- LY_CTHL_AutoDelay = dwTickCount + 1000;
- }
- break;
- case 801:
- if(dwTickCount >= LY_CTHL_AutoDelay)
- {
-
- bRunning=0;
- cBanAuTo=0;
- LY_CTHL_QDXK_VAVLE=0;
- LY_CTHL_XKTD_VAVLE = 0;
- LY_CTHL_DXK_VAVLE=0;
- LY_CTHL_AutoStep = 0;
-
- }
- break;
- case 11:
-
- if(LY_CTHL_TD_MotorStep == 0) //穿入定位完成
- {
- /* //测试拖带定位
- LY_CTHL_AutoStep = 1;
- LY_CTHL_AutoDelay = dwTickCount + 1000;
- if(cZipCnt<3)
- cZipCnt++;
- break; */
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
-
- LY_CTHL_XKDW_VAVLE = 0; //斜口上止定位关
- LY_CTHL_XKTD_VAVLE = 1; //斜口推带气缸输出
- LY_CTHL_CXSCDW_VAVLE = 1; //Y21 插销上止定位电磁阀
- cXcxcrCnt = 0;
- if(LY_CTHL_CL_MotorStep == 0)
- {
- LY_CTHL_CXQJD_VAVLE = 1;
- }
- LY_CTHL_AutoStep = 12;
- LY_CTHL_AutoDelay = dwTickCount;
- }
-
- }
- break;
- case 12:
- if(LY_CTHL_CL_MotorStep == 0) //插销前夹带回到原位
- {
- if(LY_CTHL_SF_Origin_IN)
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_AutoStep = 14;
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_PARAM_XCQJD_XCDW_DELAY;
- if(cXcxcrCnt != 1)
- {
- LY_CTHL_CXQJD_VAVLE = 1;
- }
- }
- }
- else if(dwTickCount >= (LY_CTHL_AutoDelay+100))
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_SF_Origin_ALARM,bRunning); //伺服不在原位
- }
- }
- break;
- case 14:
- if(dwTickCount >= LY_CTHL_AutoDelay)
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_XKDW_VAVLE = 1; //斜口定位
- LY_CTHL_AutoStep = 15;
- LY_CTHL_FKGD_VAVLE = 1; //方块固定
- //Y轴启动插销
- if(cXcxcrCnt != 1)
- {
- LY_CTHL_CL_MotorStep = 20; //插销定位
- }
- }
- }
- break;
- case 15:
- if(LY_CTHL_CL_MotorStep == 0) //插销定位完成
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_AutoStep = 16;
- LY_CTHL_CXDWZ_VAVLE = 1; //插销定位针(到位)电磁阀
- LY_CTHL_CXHJD_VAVLE = 1; //Y14 插销后夹带输出
- LY_CTHL_AutoDelay = dwTickCount + 10; //延时插销下移
- }
- }
- break;
- case 16:
- if(dwTickCount >= LY_CTHL_AutoDelay)
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_AutoStep = 17;
- if(LY_CTHL_PARAM_CXLY_MODE)
- LY_CTHL_CXLY_VAVLE = 0;
- else
- LY_CTHL_CXLY_VAVLE = 1; //Y06 插销下移电磁阀
- LY_CTHL_CXHJD_VAVLE = 1; //Y14 插销后夹带输出
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME; //插销下移异常时间
- }
- }
- break;
- case 17:
- if(LY_CTHL_PARAM_CXLY_MODE)
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- if(LY_CTHL_CXLY_Limit_IN)
- {
- LY_CTHL_AutoStep = 18;
- if( cXcxcrCnt==0 && !X_DRV && !cCXCONE) //拖带电机反转
- {
- AxisMovePosAccDec(X_AXIS,8000,-LY_CTHL_PARAM_TDFZ_LENTH,800,600,6,6,0);
- }
- LY_CTHL_CXQJD_VAVLE = 0; //插销前夹带电磁阀
- LY_CTHL_CXSCDW_VAVLE = 0; //Y21 插销上止定位关
- LY_CTHL_CXHJD_VAVLE = 1; //Y14 插销后夹带输出
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_PARAM_SCDWOff_CXDW_DELAY;
- }
-
- }
- else if(dwTickCount >= LY_CTHL_AutoDelay)
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_CXLY_Limit_ALARM,bRunning); //插销下移异常
-
- }
- }
- else
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_AutoStep = 18;
- if( cXcxcrCnt==0 && !X_DRV && !cCXCONE) //拖带电机反转
- {
- AxisMovePosAccDec(X_AXIS,8000,-LY_CTHL_PARAM_TDFZ_LENTH,800,600,6,6,0);
- }
- LY_CTHL_CXQJD_VAVLE = 0; //插销前夹带电磁阀
- LY_CTHL_CXSCDW_VAVLE = 0; //Y21 插销上止定位关
-
- LY_CTHL_CXHJD_VAVLE = 1; //Y14 插销后夹带输出
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_PARAM_SCDWOff_CXDW_DELAY;
- }
- }
- break;
- case 18:
- // if(START_IN_UP)
- {
- if(dwTickCount >= LY_CTHL_AutoDelay)
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_AutoStep = 19;
- cBAOHUCNT=0;
- LY_CTHL_CXHDW_VAVLE = 1; //插销后定位(插销穿入)
- LY_CTHL_AutoDelay = dwTickCount + 500; //插销穿入到位异常时间
- LY_CTHL_CL_MotorStep = 50; //合链电机回原点
- }
- }
- }
- break;
- case 19:
-
- if(LY_CTHL_CX_Limit_IN && (cXcxcrCnt<4))
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- cXcxcrCnt=0;
- cXiaoChaCNT=0;
- user_datas[100]=0;
- bZhuangLiaoOkFlg = 0;
- LY_CTHL_CXLY_VAVLE=0;
- LY_CTHL_AutoStep = 901;
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_PARAM_FKGD_DELAY; //退方块固定时间
- LY_CTHL_CXDWZ_VAVLE = 0; //插销定位针(到位)电磁阀
- }
-
- }
- else if ((dwTickCount >= LY_CTHL_AutoDelay) && (cXcxcrCnt<3) )
- {
- cXcxcrCnt++;
- LY_CTHL_CXHJD_VAVLE=0;
- LY_CTHL_CXHDW_VAVLE=0;
- if(!LY_CTHL_PARAM_CXLY_MODE)
- LY_CTHL_CXLY_VAVLE=0;
- else
- LY_CTHL_CXLY_VAVLE=1;
- if(cXcxcrCnt<3)
- {
- LY_CTHL_CXSCDW_VAVLE=1;
- LY_CTHL_AutoStep = 12;
- LY_CTHL_AutoDelay = dwTickCount + 120;
- }
- }
- else if((dwTickCount >= (LY_CTHL_AutoDelay +3000)) && !LY_CTHL_CX_Limit_IN )
- {
- cXiaoChaCNT=1;
- if(!LY_CTHL_PARAM_CXLY_MODE)
- LY_CTHL_CXLY_VAVLE=0;
- else
- LY_CTHL_CXLY_VAVLE=1;
- LY_CTHL_CXHJD_VAVLE =0;
-
- cBAOHUCNT=1;
- LY_CTHL_SetAlarmCode(LY_CTHL_CX_Limit_ALARM ,bRunning);//插销穿入到位异常
- }
-
- if( cXcxcrCnt > 2)
- {
- cXiaoChaCNT=1;
-
- LY_CTHL_CXLY_VAVLE=0;
- LY_CTHL_CXHJD_VAVLE =0;
- cBAOHUCNT=1;
- LY_CTHL_SetAlarmCode(LY_CTHL_CX_Limit_ALARM ,bRunning);//插销穿入到位异常
- }
-
- break;
-
- case 901:
- if(dwTickCount >= LY_CTHL_AutoDelay)
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_FKGD_VAVLE = 0; //方块固定退
- LY_CTHL_AutoStep = 20;
- if(!cBAOHUCNT)
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_PARAM_First_CL_DELAY;//延时一次穿入
- else
- {
- LY_CTHL_AutoDelay = dwTickCount + 1000;//手动插销保护时间
- cBAOHUCNT=0;
- }
- }
-
- }
- break;
- case 20:
- if(dwTickCount >= LY_CTHL_AutoDelay)
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_AutoStep = 21;
- LY_CTHL_First_CT_VAVLE = 1; //一次穿入电磁阀输出
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME;
- }
- }
- break;
- case 21:
- if(!LY_CTHL_First_CT_Origin_IN) //原点离开后才开始时间
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_AutoStep = 22;
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_PARAM_First_CL_Timer; //一次穿入时间
- }
- }
- else if(dwTickCount >= LY_CTHL_AutoDelay)
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_YCCRYWYC_ALARM,bRunning); //一次穿入原位异常
- }
- break;
- case 22:
- if(dwTickCount >= LY_CTHL_AutoDelay)
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_AutoStep = 23;
- LY_CTHL_MGuo_VAVLE = 0; //码勾退
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_PARAM_MGOff_XiaMo_DELAY;
- LY_CTHL_FKGD_VAVLE = 0; //方块固定退
- LY_CTHL_FKJD_VAVLE=0;
- LY_CTHL_CXHJD_VAVLE = 0; //Y14 插销后夹带打开
- LY_CTHL_CXLY_VAVLE = 0; //Y06 插销下移电磁阀关
- }
- }
- break;
- case 23:
- if(dwTickCount >= LY_CTHL_AutoDelay)
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_AutoStep = 24;
- LY_CTHL_CTXM_VAVLE = 0; //穿头下模下降
- LY_CTHL_AutoDelay = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME;
- }
- }
- break;
- case 24:
- if(!LY_CTHL_CTM_Limit_IN)
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_AutoStep = 25;
- LY_CTHL_AutoDelay = dwTickCount + 20; //一次穿入延时返回
- }
- }
- else if(dwTickCount >= LY_CTHL_AutoDelay)
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_CTM_Origin_ALARM,bRunning);//穿头下模下降异常
- }
- break;
- case 25:
- if(dwTickCount >= LY_CTHL_AutoDelay)
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- LY_CTHL_AutoStep = 26;
- LY_CTHL_First_CT_VAVLE = 0; //一次穿入电磁阀关
- LY_CTHL_AutoDelay = dwTickCount + 5;
- LY_CTHL_CXHDW_VAVLE = 0; //插销后定位(插销穿入)关闭
- //退下出错警告
- LY_CTHL_AutoDelay1 = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME;
- LY_CTHL_AutoStep1 = 1;
- }
- }
- break;
-
-
- case 26:
- if(dwTickCount >= LY_CTHL_AutoDelay)
- {
- if(!LY_CTHL_bDanBu || LY_CTHL_START_IN_UP)
- {
- if(!cCXCONE)
- {
- LY_CTHL_FKJD_VAVLE = 1; //方块夹带气缸输出
- LY_CTHL_FKGD_VAVLE = 0; //方块固定关
- LY_CTHL_AutoStep = 2;
- bCL_EN_FLG = 1;
- cXcxcrCnt=0;
- AddToTal(LY_CTHL_TOTAL_ADDR);
-
- CalProSP(LY_CTHL_SPEED_ADDR);
- if(cZipCnt<3)
- cZipCnt++;
- }
- else
- {
- bRunning=0;
- cCXCONE=0;
- LY_CTHL_AutoStep = 0;
-
- }
- }
-
- }
- break;
-
- }
- }
- //退下模后启动送拉头
- switch(LY_CTHL_AutoStep1)
- {
- case 1:
- if(cStopMode==1)
- {
- bCL_EN_FLG=1;
- cStopMode=2;
- bStop=1;
- }
- if(LY_CTHL_CTM_Origin_IN && LY_CTHL_First_CT_Origin_IN)
- {
- LY_CTHL_ZhuangLiaoStep = 1;
- LY_CTHL_TryCnt = 0;
- LY_CTHL_AutoStep1 = 0;
- LY_CTHL_MGuo_VAVLE = 0;
- LY_CTHL_JLTou_VAVLE = 0;
- }
- else if(dwTickCount >= LY_CTHL_AutoDelay1)
- {
- //穿头模和一次穿入回位异常
- if(!LY_CTHL_CTM_Origin_IN)
- LY_CTHL_SetAlarmCode(LY_CTHL_CTM_Origin_ALARM,bRunning);
- else if(!LY_CTHL_First_CT_Origin_IN)
- LY_CTHL_SetAlarmCode(LY_CTHL_First_CT_Origin_ALARM,bRunning);
- }
- break;
-
- }
-
- }
- void LY_CTHL_CheckStart(void)
- {
- //监控
- if(cBanAuTo)
- {
- LY_CTHL_PARAM_XCRU=1;
- }
- else
- {
- LY_CTHL_PARAM_XCRU=0;
- }
- //监控
- if(cCXCONE)
- {
- LY_CTHL_PARAM_CXCR=1;
- }
- else
- {
- LY_CTHL_PARAM_CXCR=0;
- }
-
- if(cXCXQXQD)
- {
- if(LY_CTHL_START_IN_UP || bStart)
- {
- bStart=0;
- if(!bRunning)
- {
- if(LY_CTHL_SafeDoor_IN )
- {
- bRunning=1;
- cXCXQXQD =0;
- LY_CTHL_TD_MotorStep = 0;
- LY_CTHL_AutoStep=11;
- }
- else
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_SafeDoor_ALARM,0); // 安全门关上才能启动
- }
- }
- }
-
-
- }
- if(cWULATOUCnt) // 无拉头警告后按
- {
- if(LY_CTHL_START_IN_UP || bStart)
- {
- bStart=0;
- if(!bRunning)
- {
- if(LY_CTHL_SafeDoor_IN)
- {
- bRunning=1;
- LY_CTHL_TryCnt=0;
- cWULATOUCnt=0;
- LY_CTHL_ZhuangLiaoStep = 1;
- LY_CTHL_AutoStep=6;
- bTuiLaTouOkFlg = 1;
- bZhuangLiaoOkFlg= 0;
- LY_CTHL_MGuo_VAVLE = 1; //判断是否已经有料
- LY_CTHL_ZhuangLiaoDelay = dwTickCount + 150;
- }
- else
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_SafeDoor_ALARM,0); // 安全门关上才能启动
- }
- }
- }
- }
- if(!LY_CTHL_SafeDoor_IN && bRunning && !cBanAuTo && !cCXCONE)
- {
- bStop=1;
- LY_CTHL_SetAlarmCode(LY_CTHL_SafeDoor_ALARM,0); // 安全门关上才能启动
- }
-
- //启动 自动启动 单一自动
- if((LY_CTHL_START_IN_UP || bStart ) && !cWULATOUCnt && !cXCXQXQD)
- {
- if(cCHULIANBAOJIN) //无链停机启动
- {
- cCHULIANBAOJIN=0;
- SetEn(X_AXIS, MOTOR_EN);
- // dwXRealPos=0;
- LY_CTHL_TD_MotorStep=10;
- if(LY_CTHL_SF_ALR_IN )
- {
- LY_CTHL_SF_ALR_CLR=1;
- LY_CTHL_bSFBBAOJIN_Delay= dwTickCount + 300;
- }
- cZipCnt=0;
- LY_CTHL_TD_MotorDelay = dwTickCount + 500;
- LY_CTHL_AutoDelay = dwTickCount + 150;
- if(GetAlarmCode(LY_CTHL_ALARM_ADDR) != 0)
- SetAlarmCode(LY_CTHL_ALARM_ADDR,0);
- }
-
- if(!bRunning)
- {
- if(LY_CTHL_SafeDoor_IN)
- {
- cStopMode=0;
- bRunning = 1;
- cWULATOUCnt=0;
- cHeLian=0;
- SetEn(X_AXIS, MOTOR_EN);
- SetEn(Y_AXIS, MOTOR_EN);
- LY_CTHL_CL_MotorStep = 40;
- LY_CTHL_AutoStep = 1;
- bTuiLaTouOkFlg = 1;
- LY_CTHL_PARAM_ZDP_AUTO= 0;
- bZhuangLiaoOkFlg= 0;
- LY_CTHL_MGuo_VAVLE = 1; //判断是否已经有料
- LY_CTHL_ZhuangLiaoDelay = dwTickCount + 150;
- // bCL_EN_FLG = 0;
- cZipCnt = 0;
- cQDXK_DXK=0;
- LY_CTHL_LianLengthCheck = 0;
- cStopInCnt = 0;
- }
- else
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_SafeDoor_ALARM,0); // 安全门关上才能启动
- }
- }
- bStart = 0;
- }
-
- //停止
- if(LY_CTHL_STOP_IN_UP || bStop)
- {
- if(cStopMode < 2)
- cStopMode++;
- if(bRunning && cStopMode==2)
- {
- cBanAuTo=0;
- cCXCONE=0;
- bRunning=0;
- cStopMode=0;
- cXcxcrCnt=0;
- cXCXQXQD=0;
- cCHULIANBAOJIN=0;
- cWULATOUCnt=0;
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- LY_CTHL_AutoStep = 0;
- LY_CTHL_CL_MotorStep =0;
- LY_CTHL_bCLFirst_Step=0;
- LY_CTHL_ZhuangLiaoStep=0;
- // LY_CTHL_XKTD_VAVLE = 0;
- cQDXK_DXK=0;
- // LY_CTHL_XKDW_VAVLE =0;
- if(LY_CTHL_SF_ALR_IN )
- {
- LY_CTHL_SF_ALR_CLR=1;
- LY_CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
- }
- if(GetAlarm(Y_AXIS)==0 || GetAlarm(X_AXIS)==0)
- {
- SetClr(Y_AXIS, MOTOR_NOALARM);
- SetClr(X_AXIS, MOTOR_NOALARM);
- LY_CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
- }
- if(GetAlarmCode(LY_CTHL_ALARM_ADDR) != 0)
- SetAlarmCode(LY_CTHL_ALARM_ADDR,0);
-
- }
- else if(!bRunning)
- {
- cBanAuTo=0;
- cCXCONE=0;
- bRunning=0;
- cXcxcrCnt=0;
- cStopMode=0;
- cCHULIANBAOJIN=0;
- cXCXQXQD=0;
- cWULATOUCnt=0;
- bCL_EN_FLG=0;
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- SetEn(X_AXIS, MOTOR_DISEN);
- SetEn(Y_AXIS, MOTOR_DISEN);
- cStopMode=0;
- cQDXK_DXK=0;
- if(LY_CTHL_SF_ALR_IN )
- {
- LY_CTHL_SF_ALR_CLR=1;
- LY_CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
- }
- if(GetAlarm(Y_AXIS)==0 || GetAlarm(X_AXIS)==0)
- {
- SetClr(Y_AXIS, MOTOR_NOALARM);
- SetClr(X_AXIS, MOTOR_NOALARM);
- LY_CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
- }
- LY_CTHL_TLiao_VAVLE=0;
- LY_CTHL_HSLiao_VAVLE =0;
- LY_CTHL_JLTou_VAVLE =0;
- LY_CTHL_XKTD_VAVLE =0;
- LY_CTHL_XKDW_VAVLE =0;
- LY_CTHL_DXK_VAVLE =0;
- LY_CTHL_CXLY_VAVLE =0;
- LY_CTHL_CTXM_VAVLE =0;
- LY_CTHL_First_CT_VAVLE =0;
- LY_CTHL_FKGD_VAVLE =0;
- LY_CTHL_CXQJD_VAVLE =0;
- LY_CTHL_CXDWZ_VAVLE =0;
- LY_CTHL_CXHJD_VAVLE =0;
- LY_CTHL_CXHDW_VAVLE =0;
- LY_CTHL_FKJD_VAVLE =0;
- LY_CTHL_HLJJ_VAVLE =0;
- LY_CTHL_CXSCDW_VAVLE =0;
- LY_CTHL_MGuo_VAVLE =0;
- LY_CTHL_QDXK_VAVLE=0;
-
- LY_CTHL_AutoStep = 0;
- LY_CTHL_ZhuangLiaoStep=0;
- LY_CTHL_TLiaoStep=0;
- LY_CTHL_TD_MotorStep=0;
- LY_CTHL_JiaLianStep =0;
- LY_CTHL_SongLiaoStep =0;
- LY_CTHL_CL_MotorStep =0;
- LY_CTHL_AutoStep1 =0;
- LY_CTHL_bFKCXTDDW_Step=0;
- LY_CTHL_bCXCR_Step=0;
- LY_CTHL_bCLFirst_Step=0;
- if(GetAlarmCode(LY_CTHL_ALARM_ADDR) != 0)
- SetAlarmCode(LY_CTHL_ALARM_ADDR,0);
- SetPos(X_AXIS, 0);
- LY_CTHL_TLiaoStep = 0;
- LY_CTHL_AutoStep = 0;
-
- //穿合链电机是往零点方向时可以停止
- if(GetDir(Y_AXIS) == DIR_N)
- AxisEgmStop(Y_AXIS);
- }
- bStop=0;
- }
-
- if(bAlarmStop)
- {
- bRunning = 0;
- bAlarmStop=0;
- cStopMode=0;
- cXcxcrCnt=0;
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- LY_CTHL_AutoStep = 0;
- cQDXK_DXK=0;
- LY_CTHL_CL_MotorStep =0;
- LY_CTHL_bCLFirst_Step=0;
- LY_CTHL_SongLiaoStep =0;
- LY_CTHL_ZhuangLiaoStep=0;
- // LY_CTHL_XKTD_VAVLE = 0;
- // LY_CTHL_XKDW_VAVLE =0;
-
- }
- }
- //拖带电机控制动作 X轴
- void LY_CTHL_TD_Motor(void) //
- {
- #if 1
- switch(LY_CTHL_TD_MotorStep)
- {
- //1步开始,纯定位, 没有穿入
- case 1:
- if(dwTickCount >= LY_CTHL_TD_MotorDelay) //给锁轴时间
- {
- bCheckEN_FLG = 1;
- LY_CTHL_CheckInX02PosBuff= 0;
- LY_CTHL_TD_MotorStep = 2;
- SetPos(X_AXIS, 0); //启动位置设为0点
- AxisContinueMoveAcc(X_AXIS,LY_CTHL_PARAM_TD_RunSpeed*2/3,DIR_P,800,600,15,15);
- LY_CTHL_X_CXPosBuff=0;
- }
- break;
- case 2://检测到过链
- if(dwXRealPos > 400) //穿入2CM后退
- {
- if(LY_CTHL_PARAM_NLSL_MODE)
- {
- LY_CTHL_XKTD_VAVLE=0;
- }
- LY_CTHL_DXK_VAVLE = 0; //顶斜口气缸关
- LY_CTHL_QDXK_VAVLE = 0;
- }
- if(dwXRealPos >= LY_CTHL_PARAM_WLCDSHEZHI_LENTH)
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机
- }
- if(LY_CTHL_FK_Check)
- {
- LY_CTHL_DXK_VAVLE = 0; //顶斜口气缸关
- LY_CTHL_QDXK_VAVLE = 0;
- LY_CTHL_XSavePosBuff = dwXRealPos;
- LY_CTHL_TD_MotorStep = 3;
- user_datas[123] = dwXRealPos - LY_CTHL_CheckInX02PosBuff;
- if((dwXRealPos >= LY_CTHL_CheckInX02PosBuff) && LY_CTHL_CheckInX02PosBuff)
- AxisMovePosAccDec(X_AXIS,LY_CTHL_PARAM_TD_RunSpeed*2/3,LY_CTHL_PARAM_FKDW_Length - (dwXRealPos- LY_CTHL_CheckInX02PosBuff)-160,
- LY_CTHL_PARAM_FKDW_Speed,LY_CTHL_PARAM_FKDW_Speed,10,30,40);
- else
- AxisMovePosAccDec(X_AXIS,LY_CTHL_PARAM_TD_RunSpeed*2/3,LY_CTHL_PARAM_FKDW_Length-120,
- LY_CTHL_PARAM_FKDW_Speed,LY_CTHL_PARAM_FKDW_Speed,10,30,40);
- }
- break;
- case 3:
- if(LY_CTHL_CXSC_Limit_IN_UP)
- {
- LY_CTHL_X_CXPosBuff = dwXRealPos;
-
- }
-
- if(!X_DRV || (dwXRealPos >= (LY_CTHL_XSavePosBuff + LY_CTHL_PARAM_FKDW_Length)))
- {
- AxisEgmStop(X_AXIS);
- LY_CTHL_TD_MotorStep =4 ;
- LY_CTHL_LianLengthAutoCheck = dwXRealPos;
- }
- break;
- case 4:
- if(LY_CTHL_PARAM_XCX_LENTH) //0的话不判断位置
- {
-
- if ((dwXRealPos - LY_CTHL_X_CXPosBuff) > (LY_CTHL_PARAM_XCX_LENTH + LY_CTHL_PARAM_XCXWC_LENTH))
- {
- // user_datas[121] = 1;
- LY_CTHL_TD_MotorStep = 0;
- LY_CTHL_SetAlarmCode(LY_CTHL_XCXXCYC_ALARM,bRunning); // 小插销行程异常
-
- }
- else if(LY_CTHL_X_CXPosBuff == 0)
- {
- // user_datas[121] = 3;
- LY_CTHL_TD_MotorStep = 0;
- LY_CTHL_SetAlarmCode(LY_CTHL_XCXXCYC_ALARM,bRunning); // 小插销行程异常
- }
- else
- {
- LY_CTHL_TD_MotorStep = 0;
- LY_CTHL_X_CXPosBuff=dwXRealPos;
-
- }
- }
- else
- {
- LY_CTHL_TD_MotorStep = 0;
- // LY_CTHL_X_CXPosBuff=dwXRealPos;
- }
- break;
-
- //穿拉头定位
- case 10:
- if(dwTickCount >= LY_CTHL_TD_MotorDelay)
- {
- LY_CTHL_TD_MotorStep = 11;
- SetPos(X_AXIS, 0); //启动位置设为0点
- dwXRealPos = 0;
- AxisContinueMoveAcc(X_AXIS,LY_CTHL_PARAM_TDCR_LowSpeed,DIR_P,200,200,5,5);
- LY_CTHL_CheckInX02PosBuff= 0;
- LY_CTHL_X_CXPosBuff=0;
- bCheckEN_FLG = 1;
- }
- break;
- case 11:
- if(LY_CTHL_PARAM_NLSL_MODE)
- LY_CTHL_Data_Buff = 600;
- else
- LY_CTHL_Data_Buff = 400;
- if(dwXRealPos > LY_CTHL_Data_Buff) //穿入2CM后退
- {
- LY_CTHL_CheckInX02PosBuff= 0;
- LY_CTHL_DXK_VAVLE = 0; //顶斜口气缸关
- LY_CTHL_QDXK_VAVLE = 0;
- if(LY_CTHL_PARAM_NLSL_MODE)
- {
- LY_CTHL_XKTD_VAVLE=0;
- }
- LY_CTHL_TD_MotorStep = 12;
- if((cZipCnt < 2) || ((LY_CTHL_LianLengthCheck < LY_CTHL_PARAM_TDCR_LowSpeedLength+300) && (LY_CTHL_PARAM_NLSL_MODE == 0)) ||
- ((LY_CTHL_LianLengthCheck < LY_CTHL_PARAM_TDCR_LowSpeedLength+600) && (LY_CTHL_PARAM_NLSL_MODE == 1)))
- AxisContinueMoveAcc(X_AXIS,LY_CTHL_PARAM_TD_RunSpeed*2/3,DIR_P,3000,600,10,10);
- else ////两段速度移动距离
- {
- if(LY_CTHL_PARAM_NLSL_MODE == 0)
- AxisMovePosAccDecNotStop(X_AXIS,LY_CTHL_PARAM_TD_RunSpeed * 1.5,LY_CTHL_LianLengthCheck-(LY_CTHL_PARAM_TDCR_LowSpeedLength),3000,LY_CTHL_PARAM_TD_RunSpeed*2/3,15,70,600);
- else //尼龙
- {
- AxisMovePosAccDecNotStop(X_AXIS,LY_CTHL_PARAM_TD_RunSpeed * 1.3,LY_CTHL_LianLengthCheck-(LY_CTHL_PARAM_TDCR_LowSpeedLength),3000,LY_CTHL_PARAM_TD_RunSpeed*2/3,12,120,850);
- }
- }
- }
- if(LY_CTHL_FK_Check) //在这里检测到说明速度还没有起来
- {
- LY_CTHL_DXK_VAVLE = 0; //顶斜口气缸关
- LY_CTHL_QDXK_VAVLE = 0;
- if(LY_CTHL_PARAM_NLSL_MODE)
- {
- LY_CTHL_XKTD_VAVLE=0;
- }
- LY_CTHL_TD_MotorDelay= dwTickCount + LY_CTHL_PARAM_FKJDSECONG_DEALY;
- LY_CTHL_LianLengthReal = dwXRealPos;
-
- LY_CTHL_XSavePosBuff = dwXRealPos;
- LY_CTHL_TD_MotorStep = 13;
- // user_datas[122] = LY_CTHL_PARAM_FKDW_Length - (dwXRealPos- LY_CTHL_CheckInX02PosBuff);
-
- if((dwXRealPos >= LY_CTHL_CheckInX02PosBuff) && LY_CTHL_CheckInX02PosBuff)
- AxisMovePosAccDec(X_AXIS,LY_CTHL_PARAM_TD_RunSpeed,LY_CTHL_PARAM_FKDW_Length - (dwXRealPos- LY_CTHL_CheckInX02PosBuff),
- LY_CTHL_PARAM_FKDW_Speed,LY_CTHL_PARAM_FKDW_Speed,10,150,10);
- else
- AxisMovePosAccDec(X_AXIS,LY_CTHL_PARAM_TD_RunSpeed,LY_CTHL_PARAM_FKDW_Length,
- LY_CTHL_PARAM_FKDW_Speed,LY_CTHL_PARAM_FKDW_Speed,10,150,10);
- //LY_CTHL_TD_MotorStep = 0;
- }
- else if(LY_CTHL_CXSC_Limit_IN_DW)
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_DK_ERROR_ALARM,bRunning);
- }
- else if(LY_CTHL_CXSC_Limit_IN_UP)
- {
- AxisMovePosAccDec(X_AXIS,LY_CTHL_PARAM_TD_RunSpeed * 2/3,LY_CTHL_PARAM_FKDW_Length,
- LY_CTHL_PARAM_FKDW_Speed,LY_CTHL_PARAM_FKDW_Speed,10,150,10);
- }
- break;
- case 12://检测到过链
- if(cZipCnt<2)
- {
- if(dwXRealPos >= LY_CTHL_PARAM_WLCDSHEZHI_LENTH)
- {
- if(bRunning)
- {
- cCHULIANBAOJIN=1;
- SetEn(X_AXIS, MOTOR_DISEN);
- AxisEgmStop(X_AXIS);
- SetPos(X_AXIS, 0); //启动位置设为0点
- // dwXRealPos=0;
- }
- LY_CTHL_SetAlarmCode(LY_CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机
- }
- }
- else
- {
- if(dwXRealPos >= LY_CTHL_LianLengthAutoCheckERROR + LY_CTHL_PARAM_WUCD_LENTH)
- {
- if(bRunning)
- {
- cCHULIANBAOJIN=1;
- SetEn(X_AXIS, MOTOR_DISEN);
- AxisEgmStop(X_AXIS);
- SetPos(X_AXIS, 0); //启动位置设为0点
- // dwXRealPos=0;
- }
- LY_CTHL_SetAlarmCode(LY_CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机
- }
-
- }
- if(LY_CTHL_FK_Check)
- {
- LY_CTHL_TD_MotorDelay= dwTickCount + LY_CTHL_PARAM_FKJDSECONG_DEALY;
- LY_CTHL_LianLengthReal = dwXRealPos;
- if(cZipCnt == 1)
- user_datas[122] = dwXRealPos;
- // user_datas[121] = dwXRealPos;
- if((cZipCnt == 1) || dwXRealPos < (LY_CTHL_LianLengthCheck*0.85) || dwXRealPos>(LY_CTHL_LianLengthCheck*1.2))
- LY_CTHL_LianLengthCheck = dwXRealPos;
-
- LY_CTHL_LianLengthCheckOLD=LY_CTHL_LianLengthNEWCheck;
- LY_CTHL_LianLengthNEWCheck = dwXRealPos;
- //break;
- LY_CTHL_XSavePosBuff = dwXRealPos;
- LY_CTHL_TD_MotorStep = 13;
- // user_datas[122] = LY_CTHL_PARAM_FKDW_Length - (dwXRealPos- LY_CTHL_CheckInX02PosBuff);
-
- if((dwXRealPos >= LY_CTHL_CheckInX02PosBuff) && LY_CTHL_CheckInX02PosBuff)
- AxisMovePosAccDec(X_AXIS,LY_CTHL_PARAM_TD_RunSpeed,LY_CTHL_PARAM_FKDW_Length - (dwXRealPos- LY_CTHL_CheckInX02PosBuff)-50,
- LY_CTHL_PARAM_FKDW_Speed,LY_CTHL_PARAM_FKDW_Speed,10,150,40);
- else
- AxisMovePosAccDec(X_AXIS,LY_CTHL_PARAM_TD_RunSpeed,LY_CTHL_PARAM_FKDW_Length-50,
- LY_CTHL_PARAM_FKDW_Speed,LY_CTHL_PARAM_FKDW_Speed,10,150,40);
- //LY_CTHL_TD_MotorStep = 0;
- }
- else if(LY_CTHL_CXSC_Limit_IN_DW)
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_DK_ERROR_ALARM,bRunning);
- }
- else if(LY_CTHL_CXSC_Limit_IN_UP)
- {
- AxisMovePosAccDec(X_AXIS,LY_CTHL_PARAM_TD_RunSpeed * 2/3,LY_CTHL_PARAM_FKDW_Length,
- LY_CTHL_PARAM_FKDW_Speed,LY_CTHL_PARAM_FKDW_Speed,10,150,40);
- }
- break;
- case 13:
- if(dwTickCount >= LY_CTHL_TD_MotorDelay)
- {
- LY_CTHL_FKJD_VAVLE = 1; //方块夹带气缸输出
-
- }
- if(LY_CTHL_CXSC_Limit_IN_UP)
- {
- LY_CTHL_X_CXPosBuff = dwXRealPos;
-
- }
- if((!X_DRV ))// || (dwXRealPos >= (LY_CTHL_XSavePosBuff + LY_CTHL_PARAM_FKDW_Length)))
- {
- // user_datas[123] = dwXRealPos-LY_CTHL_CheckInX02PosBuff;
- AxisEgmStop(X_AXIS);
- LY_CTHL_TD_MotorStep = 14;
- /* if(cZipCnt>2)
- {
- if(((LY_CTHL_LianLengthNEWCheck+LY_CTHL_LianLengthCheckOLD)*0.5) < (LY_CTHL_LianLengthCheck*0.95))
- {
- cZipCnt=0;
- cHeLian=1;
- }
- }*/
- if(cZipCnt==1)
- LY_CTHL_LianLengthAutoCheckERROR = dwXRealPos;
-
- if(cZipCnt==1)
- LY_CTHL_LianLengSave=dwXRealPos;
- LY_CTHL_LianLengthAutoCheck = dwXRealPos;
- // user_datas[122] = dwXRealPos-LY_CTHL_X_CXPosBuff;
- }
- break;
- case 14:
-
- if(LY_CTHL_PARAM_XCX_LENTH) //0的话不判断位置
- {
-
- if ((dwXRealPos - LY_CTHL_X_CXPosBuff) > (LY_CTHL_PARAM_XCX_LENTH + LY_CTHL_PARAM_XCXWC_LENTH))
- {
- // user_datas[121] = 1;
- LY_CTHL_TD_MotorStep = 0;
- cXCXQXQD=1;
- LY_CTHL_SetAlarmCode(LY_CTHL_XCXXCYC_ALARM,bRunning); // 小插销行程异常
-
- }
- else if((dwXRealPos - LY_CTHL_X_CXPosBuff) < (LY_CTHL_PARAM_XCX_LENTH - LY_CTHL_PARAM_XCXWC_LENTH))
- {
- // user_datas[121] = 2;
- LY_CTHL_TD_MotorStep = 0;
- cXCXQXQD=1;
- LY_CTHL_SetAlarmCode(LY_CTHL_XCXXCYC_ALARM,bRunning); // 小插销行程异常
- }
- else if(LY_CTHL_X_CXPosBuff == 0)
- {
- // user_datas[121] = 3;
- LY_CTHL_TD_MotorStep = 0;
- cXCXQXQD=1;
- LY_CTHL_SetAlarmCode(LY_CTHL_XCXXCYC_ALARM,bRunning); // 小插销行程异常
- }
- else
- {
-
- LY_CTHL_TD_MotorStep = 15;
- }
- }
- else
- {
-
- LY_CTHL_TD_MotorStep = 15;
- }
- break;
- case 15:
- // if(LY_CTHL_START_IN_UP)
- LY_CTHL_TD_MotorStep = 0;
-
-
- }
- #endif
- }
- //穿入电机动作,Y轴
- void LY_CTHL_CL_Motor(void) //
- {
- #if 0
- //1步开始, 穿链长度开始
- //20步开始 插销定位开始
- //40步,回原点为
- switch(LY_CTHL_CL_MotorStep)
- {
- //穿入长度位置
- case 1:
-
- if(dwTickCount >= LY_CTHL_CL_MotorDelay)
- {
- // SetDir(Y_AXIS, DIR_P);
- if(LY_CTHL_PARAM_CL_Length >= LY_CTHL_PARAM_CL_MAX_Length)
- LY_CTHL_PARAM_CL_Length = LY_CTHL_PARAM_CL_MAX_Length;
- //穿入长度要根据检测长度减动一次穿入的1CM,反转的2CM
- if((LY_CTHL_LianLengthAutoCheck < (LY_CTHL_PARAM_CL_Length + 500 )) && bRunning)
- AxisMovePosAccDec(Y_AXIS,LY_CTHL_PARAM_CL_Speed,(LY_CTHL_LianLengthAutoCheck-500),800,600,6,6,50);
- else
- AxisMovePosAccDec(Y_AXIS,LY_CTHL_PARAM_CL_Speed,(LY_CTHL_PARAM_CL_Length-100),800,600,6,6,50);
- LY_CTHL_CL_MotorStep = 2;
- }
- break;
- case 2:
- if((dwYRealPos + 120) > LY_CTHL_PARAM_CL_Length)
- LY_CTHL_HLJJ_VAVLE = 0;
- if(!Y_DRV || dwYRealPos >= LY_CTHL_PARAM_CL_Length)
- {
- LY_CTHL_HLJJ_VAVLE = 0;
- LY_CTHL_CL_MotorStep = 3;
- bCL_OK_FLG = 1;
- LY_CTHL_CL_MotorDelay = dwTickCount + 40; //延时数控回零点
- }
- break;
- case 3:
- if(dwTickCount >= LY_CTHL_CL_MotorDelay)
- {
- LY_CTHL_CL_MotorStep = 50;
- }
- break;
- //夹带插销到穿入位置
- case 20: //此处最好算好定位长度相减
- if(cXcxcrCnt > 0)
- {
- LY_CTHL_LENTH = LY_CTHL_PARAM_CXDW_Length + 30;
- LY_CTHL_CL_MotorStep = 21;
- }
- else
- {
- LY_CTHL_LENTH = LY_CTHL_PARAM_CXDW_Length ;
- LY_CTHL_CL_MotorStep = 21;
- }
- break;
- case 21:
- SetDir(Y_AXIS, DIR_P);
- AxisMovePosAccDec(Y_AXIS,8000,LY_CTHL_LENTH,800,500,6,6,50);
- LY_CTHL_CL_MotorStep = 22;
- break;
- case 22:
- if(!Y_DRV || (dwYRealPos >= LY_CTHL_LENTH)) //插销定位已经结束
- {
- AxisEgmStop(Y_AXIS);
- LY_CTHL_YsaveCXDWPosBuff = 0;
- LY_CTHL_CL_MotorStep = 0;
- }
- break;
- //回原点开始
- case 40:
- //要先离开
- if(LY_CTHL_SF_Origin_IN && (GetEn(Y_AXIS) == MOTOR_DISEN))
- {
- LY_CTHL_CL_MotorStep = 41;
- SetDir(Y_AXIS, DIR_P);
- }
- else
- {
- SetDir(Y_AXIS, DIR_N);
- LY_CTHL_CL_MotorStep = 46;
-
- }
- if(GetEn(Y_AXIS) == MOTOR_DISEN)
- {
- SetEn(Y_AXIS, MOTOR_EN);
- LY_CTHL_CL_MotorDelay = dwTickCount + 260; //锁轴时间
-
- }
- else //已经锁轴
- LY_CTHL_CL_MotorDelay = dwTickCount + 5;
- break;
- case 41:
- if(dwTickCount >= LY_CTHL_CL_MotorDelay)
- {
- LY_CTHL_CL_MotorStep = 42;
- AxisContinueMoveAcc(Y_AXIS,5000,DIR_P,1000,800,15,15);
- }
- break;
- case 42:
- if(!LY_CTHL_SF_Origin_IN)
- {
- LY_CTHL_CL_MotorStep = 43;
- LY_CTHL_YSavePosBuff = dwYRealPos;
- }
- break;
- case 43:
- //离开原点5CM停止
- if((dwYRealPos - LY_CTHL_YSavePosBuff) >= 800)
- {
- AxisEgmStop(Y_AXIS);
- LY_CTHL_CL_MotorStep = 46; //留空几步备用
- LY_CTHL_CL_MotorDelay = dwTickCount + 50;
- }
- break;
- case 46:
- if(dwTickCount >= LY_CTHL_CL_MotorDelay)
- {
- LY_CTHL_CL_MotorStep = 47; //检测回到原位
- if(!LY_CTHL_SF_Origin_IN)
- AxisContinueMoveAcc(Y_AXIS,1500,DIR_N,1000,800,15,10);
- }
- break;
- case 47:
- if(LY_CTHL_SF_Origin_IN)
- {
- LY_CTHL_CL_MotorStep = 0;
- AxisEgmStop(Y_AXIS);
- SetPos(Y_AXIS, 0);
- }
- break;
- //数控回原位 (条件为自动工作,SON一直没有松开)
- case 50:
- if(dwYRealPos > 150)
- { //回零速度
- SetDir(Y_AXIS, DIR_N);
- AxisMovePosAccDecNotStop(Y_AXIS,LY_CTHL_PARAM_CL_Back_O_Speed,-dwYRealPos + 150,5000,2000,10,50,50);
-
-
- }
- else
- AxisContinueMoveAcc(Y_AXIS,1000,DIR_N,1000,1000,15,15);
- LY_CTHL_CL_MotorStep = 51;
- break;
- case 51:
- if(LY_CTHL_SF_Origin_IN)
- {
- LY_CTHL_CL_MotorStep = 0;
- AxisEgmStop(Y_AXIS);
- SetPos(Y_AXIS, 0);
- }
- break;
-
- }
- #endif
- #if 1
- //1步开始, 穿链长度开始
- //20步开始 插销定位开始
- //40步,回原点为
- switch(LY_CTHL_CL_MotorStep)
- {
- //穿入长度位置
- case 1:
- if(dwTickCount >= LY_CTHL_CL_MotorDelay)
- {
- if(!LY_CTHL_PARAM_AUTO_MODE)
- {
- //穿入长度要根据检测长度减动一次穿入的1CM,反转的2CM
- if(LY_CTHL_LianLengthAutoCheck <= 700)
- {
- LY_CTHL_Y_CL_Length=LY_CTHL_LianLengthAutoCheck-200;
- if(LY_CTHL_Y_CL_Length<10)
- LY_CTHL_Y_CL_Length=200;
- if(LY_CTHL_Y_CL_Length >= LY_CTHL_PARAM_CL_MAX_Length)
- LY_CTHL_Y_CL_Length = LY_CTHL_PARAM_CL_MAX_Length-40;
- AxisMovePosAccDec(Y_AXIS,LY_CTHL_PARAM_CL_Speed,LY_CTHL_Y_CL_Length,800,600,6,6,50);
- }
- else if(LY_CTHL_LianLengthAutoCheck>700)
- {
-
- user_datas[123] =LY_CTHL_LianLengthAutoCheck;
- if(cZipCnt>1)
- LY_CTHL_Y_CL_Length=LY_CTHL_LianLengSave-LY_CTHL_PARAM_HLTSY_LENTH-300;
- else
- {
- if(cZipCnt<1)
- LY_CTHL_Y_CL_Length=LY_CTHL_LianLengthAutoCheck-LY_CTHL_PARAM_HLTSY_LENTH-100;
- else
- LY_CTHL_Y_CL_Length=LY_CTHL_LianLengthAutoCheck-LY_CTHL_PARAM_HLTSY_LENTH-300;
- }
- if(cZipCnt>1)
- {
- if(LY_CTHL_LianLengthAutoCheck<(LY_CTHL_LianLengSave*0.7))
- {
- LY_CTHL_Y_CL_Length=LY_CTHL_LianLengthAutoCheck-LY_CTHL_PARAM_HLTSY_LENTH-300;
- cZipCnt=0;
- }
- }
- if(LY_CTHL_Y_CL_Length<200)
- LY_CTHL_Y_CL_Length=200;
- if(LY_CTHL_Y_CL_Length >= LY_CTHL_PARAM_CL_MAX_Length)
- LY_CTHL_Y_CL_Length = LY_CTHL_PARAM_CL_MAX_Length-40;
- AxisMovePosAccDec(Y_AXIS,LY_CTHL_PARAM_CL_Speed,LY_CTHL_Y_CL_Length,800,600,6,6,50);
-
- }
-
- }
-
- else
- {
- if(LY_CTHL_PARAM_CL_Length >= LY_CTHL_PARAM_CL_MAX_Length)
- LY_CTHL_PARAM_CL_Length = LY_CTHL_PARAM_CL_MAX_Length;
- //穿入长度要根据检测长度减动一次穿入的1CM,反转的2CM
- if((LY_CTHL_LianLengthAutoCheck < (LY_CTHL_PARAM_CL_Length + 500 )) && bRunning)
- AxisMovePosAccDec(Y_AXIS,LY_CTHL_PARAM_CL_Speed,(LY_CTHL_LianLengthAutoCheck-500),800,600,6,6,50);
- else
- AxisMovePosAccDec(Y_AXIS,LY_CTHL_PARAM_CL_Speed,(LY_CTHL_PARAM_CL_Length-100),800,600,6,6,50);
-
- }
- LY_CTHL_CL_MotorStep = 2;
- }
- break;
- case 2:
- if(LY_CTHL_PARAM_AUTO_MODE)
- {
- if((dwYRealPos + 120) > LY_CTHL_PARAM_CL_Length)
- LY_CTHL_HLJJ_VAVLE = 0;
- if(!Y_DRV || dwYRealPos >= LY_CTHL_PARAM_CL_Length)
- {
- LY_CTHL_HLJJ_VAVLE = 0;
- LY_CTHL_CL_MotorStep = 3;
- bCL_OK_FLG = 1;
- LY_CTHL_CL_MotorDelay = dwTickCount + 30; //延时数控回零点
- }
- }
- else
- {
- if((dwYRealPos + 120) > LY_CTHL_Y_CL_Length)
- LY_CTHL_HLJJ_VAVLE = 0;
- if(!Y_DRV || dwYRealPos >= LY_CTHL_Y_CL_Length)
- {
- LY_CTHL_HLJJ_VAVLE = 0;
- LY_CTHL_CL_MotorStep = 3;
- bCL_OK_FLG = 1;
- LY_CTHL_CL_MotorDelay = dwTickCount + 30; //延时数控回零点
- }
-
- }
- break;
- case 3:
- if(dwTickCount >= LY_CTHL_CL_MotorDelay)
- {
- LY_CTHL_CL_MotorStep = 50;
- }
- break;
- //夹带插销到穿入位置
- case 20: //此处最好算好定位长度相减
- if(cXcxcrCnt > 0)
- {
- if(cXcxcrCnt==1)
- LY_CTHL_LENTH = LY_CTHL_PARAM_CXDW_Length+LY_CTHL_PARAM_TDFZ_LENTH + 50;
- if(cXcxcrCnt>1)
- LY_CTHL_LENTH = LY_CTHL_PARAM_CXDW_Length +LY_CTHL_PARAM_TDFZ_LENTH + 120;
- LY_CTHL_CL_MotorStep = 21;
- }
- else
- {
- LY_CTHL_LENTH = LY_CTHL_PARAM_CXDW_Length ;
- LY_CTHL_CL_MotorStep = 21;
- }
- break;
- case 21:
- SetDir(Y_AXIS, DIR_P);
- LY_CTHL_Y_CL_PosBuff = dwYRealPos;
- AxisMovePosAccDec(Y_AXIS,20000,LY_CTHL_LENTH,800,500,6,6,50);
- LY_CTHL_CL_MotorStep = 22;
- break;
- case 22:
- if(dwYRealPos >= LY_CTHL_Y_CL_PosBuff+LY_CTHL_LENTH-LY_CTHL_PARAM_CXDW_Length) //插销定位已经结束
- {
- LY_CTHL_CXSCDW_VAVLE = 1;
- }
-
- if(!Y_DRV || (dwYRealPos >= LY_CTHL_Y_CL_PosBuff+LY_CTHL_LENTH)) //插销定位已经结束
- {
- AxisEgmStop(Y_AXIS);
- LY_CTHL_YsaveCXDWPosBuff = 0;
- LY_CTHL_CL_MotorStep = 0;
- }
-
- break;
- case 30:
- LY_CTHL_YsaveCXDWPosBuff = 0;
- LY_CTHL_CL_MotorStep = 0;
- break;
- //回原点开始
- case 40:
- //要先离开
- if(LY_CTHL_SF_Origin_IN && (GetEn(Y_AXIS) == MOTOR_DISEN))
- {
- LY_CTHL_CL_MotorStep = 41;
- SetDir(Y_AXIS, DIR_P);
- }
- else
- {
- SetDir(Y_AXIS, DIR_N);
- LY_CTHL_CL_MotorStep = 46;
-
- }
- if(GetEn(Y_AXIS) == MOTOR_DISEN)
- {
- SetEn(Y_AXIS, MOTOR_EN);
- LY_CTHL_CL_MotorDelay = dwTickCount + 260; //锁轴时间
-
- }
- else //已经锁轴
- LY_CTHL_CL_MotorDelay = dwTickCount + 5;
- break;
- case 41:
- if(dwTickCount >= LY_CTHL_CL_MotorDelay)
- {
- LY_CTHL_CL_MotorStep = 42;
- AxisContinueMoveAcc(Y_AXIS,5000,DIR_P,1000,800,15,15);
- }
- break;
- case 42:
- if(!LY_CTHL_SF_Origin_IN)
- {
- LY_CTHL_CL_MotorStep = 43;
- LY_CTHL_YSavePosBuff = dwYRealPos;
- }
- break;
- case 43:
- //离开原点5CM停止
- if((dwYRealPos - LY_CTHL_YSavePosBuff) >= 800)
- {
- AxisEgmStop(Y_AXIS);
- LY_CTHL_CL_MotorStep = 46; //留空几步备用
- LY_CTHL_CL_MotorDelay = dwTickCount + 50;
- }
- break;
- case 46:
- if(dwTickCount >= LY_CTHL_CL_MotorDelay)
- {
- LY_CTHL_CL_MotorStep = 47; //检测回到原位
- if(!LY_CTHL_SF_Origin_IN)
- AxisContinueMoveAcc(Y_AXIS,1500,DIR_N,1000,800,15,10);
- }
- break;
- case 47:
- if(LY_CTHL_SF_Origin_IN)
- {
- LY_CTHL_CL_MotorStep = 0;
- AxisEgmStop(Y_AXIS);
- SetPos(Y_AXIS, 0);
- }
- break;
- //数控回原位 (条件为自动工作,SON一直没有松开)
- case 50:
- if(dwYRealPos > 151)
- { //回零速度
-
- SetDir(Y_AXIS, DIR_N);
- AxisMovePosAccDecNotStop(Y_AXIS,LY_CTHL_PARAM_CL_Back_O_Speed,-dwYRealPos + 150,5000,2000,10,50,100);
- }
- else
- AxisContinueMoveAcc(Y_AXIS,1000,DIR_N,1000,1000,15,15);
- LY_CTHL_CL_MotorStep = 51;
- break;
- case 51:
- if(LY_CTHL_SF_Origin_IN)
- {
- LY_CTHL_CL_MotorStep = 0;
- AxisEgmStop(Y_AXIS);
- SetPos(Y_AXIS, 0);
- }
- break;
-
- }
- #endif
- }
- //装拉头动作
- void LY_CTHL_ZLT_Step(void)
- {
- switch(LY_CTHL_ZhuangLiaoStep)
- {
- case 1:
-
- if(bZhuangLiaoOkFlg)
- {
- LY_CTHL_ZhuangLiaoStep = 0;
- }
- else if(dwTickCount >= LY_CTHL_ZhuangLiaoDelay)
- {
-
- if(((LY_CTHL_MGuo_VAVLE && LY_CTHL_LTou_Check) || (!LY_CTHL_MGuo_VAVLE)) && !LY_CTHL_JLiao_Limit_IN )//说明没有拉头
- {
- //穿头模和一次穿入必须在原位,接料不能有输出
- if(LY_CTHL_CTM_Origin_IN && !LY_CTHL_CTXM_VAVLE)
- {
- LY_CTHL_First_CT_VAVLE = 0;
- LY_CTHL_JLTou_VAVLE = 0;
- LY_CTHL_MGuo_VAVLE = 0;
- if(LY_CTHL_TLiaoStep == 0)
- {
- LY_CTHL_TLiaoStep = 1;
- LY_CTHL_ZhuangLiaoStep = 2;
- }
- }
-
- else if(dwTickCount >= LY_CTHL_ZhuangLiaoDelay + 500) //警告横送拉必须保证穿头模和一次穿入在原位,接料不能有输出
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_CTM_Origin_ALARM,bRunning);
-
- }
- }
- else //已经有拉头直接跳到结束
- {
- LY_CTHL_ZhuangLiaoDelay = dwTickCount + 40;
- LY_CTHL_ZhuangLiaoStep = 9;
- }
- }
- break;
- case 2:
- if(LY_CTHL_TLiaoStep == 0) //推料已经完成
- {
- LY_CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
- LY_CTHL_JLTou_VAVLE = 0;
- LY_CTHL_MGuo_VAVLE = 0;
- if(!LY_CTHL_JLiao_Limit_IN && LY_CTHL_First_CT_Origin_IN)
- {
- LY_CTHL_ZhuangLiaoStep = 3;
- LY_CTHL_HSLiao_VAVLE = 1;
- }
- }
- break;
- case 3:
- //横送到位
- if(LY_CTHL_HSLiao_Limit_IN && !LY_CTHL_HSLiao_Origin_IN)
- {
- LY_CTHL_ZhuangLiaoStep = 4;
- // LY_CTHL_JLTou_VAVLE = 1; //接拉头输出,不需要时间
- LY_CTHL_ZhuangLiaoDelay = dwTickCount + 5;
- }
- else if(dwTickCount >= LY_CTHL_ZhuangLiaoDelay)
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_HSLiao_Limit_ALARM,bRunning);
- }
- break;
- case 4:
- if(dwTickCount >= LY_CTHL_ZhuangLiaoDelay)
- {
- LY_CTHL_ZhuangLiaoStep = 5;
- LY_CTHL_JLTou_VAVLE = 1; //接拉头输出,不需要时间
- LY_CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
- }
- break;
- case 5:
- if(LY_CTHL_JLiao_Limit_IN)
- {
- LY_CTHL_ZhuangLiaoStep = 6;
- LY_CTHL_ZhuangLiaoDelay = dwTickCount + LY_CTHL_PARAM_MaGou_DELAY;
- }
- else if(dwTickCount >= LY_CTHL_ZhuangLiaoDelay) //接拉头(接料)到们信号异常
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_JLiao_Limit_ALARM,bRunning);
- }
- break;
- case 6:
- if(dwTickCount >= LY_CTHL_ZhuangLiaoDelay) //延时码勾
- {
- LY_CTHL_MGuo_VAVLE = 1; //码勾输出
- LY_CTHL_ZhuangLiaoStep = 7;
- LY_CTHL_ZhuangLiaoDelay = dwTickCount + LY_CTHL_PARAM_HSLiaoOff_DELAY;
- bTuiLaTouOkFlg = 0; //认为拉头没法带出
- }
- break;
- case 7:
- if(dwTickCount >= LY_CTHL_ZhuangLiaoDelay)
- {
- LY_CTHL_HSLiao_VAVLE = 0;
- LY_CTHL_ZhuangLiaoStep = 8;
- LY_CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
- }
- break;
- case 8:
- if(!LY_CTHL_HSLiao_Limit_IN) //
- {
- LY_CTHL_ZhuangLiaoStep = 9;
- LY_CTHL_TLiaoStep = 1; //推拉头
- LY_CTHL_ZhuangLiaoDelay = dwTickCount + LY_CTHL_PARAM_CheckLT_DELAY; //也是给下模能上升的时间
- }
- else if (LY_CTHL_LTou_Check) //时间内信号这说明没有拉头
- {
- //重送3次
- LY_CTHL_TryCnt++;
- LY_CTHL_JLTou_VAVLE = 0; //接拉头关
- LY_CTHL_MGuo_VAVLE = 0; //码勾打开
- if(LY_CTHL_TryCnt >= 3)
- {
- LY_CTHL_TryCnt = 0;
- bZhuangLiaoOkFlg = 0;
- LY_CTHL_ZhuangLiaoStep = 0;
- LY_CTHL_SetAlarmCode(LY_CTHL_ZLT_ALARM,bRunning);
- cWULATOUCnt=1;
- }
- else
- {
- LY_CTHL_ZhuangLiaoStep = 20; //重新送3次
- LY_CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
- }
- }
- else if(dwTickCount >= LY_CTHL_ZhuangLiaoDelay)
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_HSLiao_Limit_ALARM,bRunning);
- if(bRunning)
- {
- LY_CTHL_ZhuangLiaoStep = 0 ;
- }
- }
- break;
- case 9:
- if(LY_CTHL_LTou_Check) //时间内信号这说明没有拉头
- {
- LY_CTHL_TryCnt++;
- LY_CTHL_JLTou_VAVLE = 0; //接拉头关
- LY_CTHL_MGuo_VAVLE = 0; //码勾打开
- if(LY_CTHL_TryCnt >= 3)
- {
- LY_CTHL_TryCnt=0;
- bZhuangLiaoOkFlg = 0;
- LY_CTHL_ZhuangLiaoStep = 0;
- LY_CTHL_SetAlarmCode(LY_CTHL_ZLT_ALARM,bRunning);
- cWULATOUCnt=1;
- }
- else
- {
- LY_CTHL_ZhuangLiaoStep = 20; //重新送3次
- LY_CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
- }
- }
- else if(dwTickCount >= LY_CTHL_ZhuangLiaoDelay)
- {
- bZhuangLiaoOkFlg = 1;
- LY_CTHL_ZhuangLiaoStep = 0;
- }
- break;
- //装拉头不成攻重新送拉头
- case 20:
- if(LY_CTHL_HSLiao_Origin_IN)
- {
- LY_CTHL_JLTou_VAVLE = 0; //接拉头关
- LY_CTHL_MGuo_VAVLE = 0; //码勾打开
- LY_CTHL_ZhuangLiaoStep = 1;
- LY_CTHL_ZhuangLiaoDelay = dwTickCount + 10;
- }
- else if(dwTickCount >= LY_CTHL_ZhuangLiaoDelay)
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_HSLiao_Origin_ALARM,bRunning);
- }
- break;
- }
- }
- void LY_CTHL_TLiao_Step(void)
- {
- switch(LY_CTHL_TLiaoStep)
- {
- case 1:
- if(bTuiLaTouOkFlg)
- {
- LY_CTHL_TLiaoStep = 0;
- }
- else
- {
- LY_CTHL_TLiao_Delay = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME;
- LY_CTHL_TLiaoStep = 2;
- }
- break;
- case 2: //横送料在原位
- if(LY_CTHL_HSLiao_Origin_IN && !LY_CTHL_HSLiao_Limit_IN && !LY_CTHL_HSLiao_VAVLE)
- {
- LY_CTHL_TLiao_VAVLE = 1; //推料(推拉头)输出
- LY_CTHL_TLiao_Delay = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME;
- LY_CTHL_TLiaoStep = 3;
- }
- else if(dwTickCount >= LY_CTHL_TLiao_Delay)
- LY_CTHL_SetAlarmCode(LY_CTHL_HSLiao_Origin_ALARM,1);
- break;
- case 3: //推拉头到位
- if(LY_CTHL_TLiao_Limit_IN)
- {
- LY_CTHL_TLiao_Delay = dwTickCount + LY_CTHL_PARAM_TLiao_BACKDELAY;
- LY_CTHL_TLiaoStep = 4;
- }
- else if(dwTickCount >= LY_CTHL_TLiao_Delay)
- LY_CTHL_SetAlarmCode(LY_CTHL_TLiao_Limit_ALARM,1);
- break;
- case 4:
- if(dwTickCount >= LY_CTHL_TLiao_Delay)
- {
- LY_CTHL_TLiao_VAVLE = 0;
- LY_CTHL_TLiao_Delay = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME;
- LY_CTHL_TLiaoStep = 5;
- }
- break;
- case 5:
- if(!LY_CTHL_TLiao_Limit_IN)
- {
- LY_CTHL_TLiaoStep = 0;
- LY_CTHL_TLiao_Delay = dwTickCount;
- bTuiLaTouOkFlg = 1;
- }
- else if(dwTickCount >= LY_CTHL_TLiao_Delay)
- LY_CTHL_SetAlarmCode(LY_CTHL_TLiao_Limit_ALARM,0);
- break;
- }
-
- }
- //振动盘控制
- void LY_CTHL_ZhenDongAction(void)
- {
-
- #if 0
- if((LY_CTHL_bDLP || bRunning) && LY_CTHL_ZhenDongPian_OUT && LY_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)
- // {
- switch(LY_CTHL_PARAM_ZDP_AUTO)
- {
- case 0:
- if(LY_CTHL_ZhenDongPian_OUT == 0)
- {
- if(!LY_CTHL_ZDP_IN)
- {
- if(dwTickCount >= LY_CTHL_ZDP_Delay)
- {
- LY_CTHL_ZhenDongPian_OUT = 1;
- LY_CTHL_ZDP_Delay = dwTickCount + LY_CTHL_PARAM_ZDP_Stop_TIME;//LY_CTHL_PARAM_ZDP_Stop_TIME + 50;
- }
- }
- else
- {
- LY_CTHL_ZDP_Delay = dwTickCount + LY_CTHL_PARAM_ZDP_Start_TIME;//LY_CTHL_PARAM_ZDP_Start_TIME;
- }
- }
- else
- {
- if(LY_CTHL_ZDP_IN)
- {
- if(dwTickCount >= LY_CTHL_ZDP_Delay)
- {
- LY_CTHL_ZhenDongPian_OUT = 0;
- LY_CTHL_ZDP_Delay = dwTickCount + LY_CTHL_PARAM_ZDP_Start_TIME;//LY_CTHL_PARAM_ZDP_Start_TIME
- }
- }
- else
- LY_CTHL_ZDP_Delay = dwTickCount + LY_CTHL_PARAM_ZDP_Stop_TIME;//LY_CTHL_PARAM_ZDP_Stop_TIME+ 50;
- }
- break;
- case 1:
- LY_CTHL_ZhenDongPian_OUT=0;
- LY_CTHL_ZDP_Delay = dwTickCount;
- break;
- case 2:
- LY_CTHL_ZhenDongPian_OUT=1;
- LY_CTHL_ZDP_Delay = dwTickCount;
- break;
- }
- if(LY_CTHL_ZhenDongPian_OUT)
- {
- if(LY_CTHL_PARAM_LaPian_EN)
- {
- if(dwTickCount >= LY_CTHL_DLP_Delay)
- {
-
- LY_CTHL_DaLaPian_OUT = !LY_CTHL_DaLaPian_OUT;
-
- if(LY_CTHL_DaLaPian_OUT)
- LY_CTHL_DLP_Delay = dwTickCount + LY_CTHL_PARAM_DaLaPian;
- else
- LY_CTHL_DLP_Delay = dwTickCount + LY_CTHL_PARAM_DaLaPian+50;
- }
- }
- else
- LY_CTHL_DaLaPian_OUT = 0;
- }
- else
- LY_CTHL_DaLaPian_OUT = 0;
- #endif
- }
- void LY_CTHL_bFKCXTDDWStep(void)
- {
- switch(LY_CTHL_bFKCXTDDW_Step)
- {
- case 1:
- LY_CTHL_FKJD_VAVLE = 1; //方块夹带气缸输出
- LY_CTHL_XKTD_VAVLE = 1; //斜口推带气缸输出
- LY_CTHL_FKGD_VAVLE = 0; //方块固定关
- LY_CTHL_bFKCXTDDW_Step = 2;
-
- break;
- case 2:
- if(!LY_CTHL_XKTD_Origin_IN) //斜口推带原位离开
- {
- LY_CTHL_bFKCXTDDW_Step = 3;
- LY_CTHL_bFKCXTDDW_Delay = dwTickCount + 20;
- }
- break;
- case 3://斜口定位
- if(dwTickCount >= LY_CTHL_bFKCXTDDW_Delay)
- {
- LY_CTHL_XKDW_VAVLE = 1;
- LY_CTHL_bFKCXTDDW_Step = 4;
- // LY_CTHL_bFKCXTDDW_Delay = dwTickCount + LY_CTHL_PARAM_XKDW_TD_DELAY;
- }
- break;
- case 4:
- if(dwTickCount >= LY_CTHL_bFKCXTDDW_Delay)
- {
- AxisContinueMoveAcc(X_AXIS,1200,DIR_P,800,600,10,10);//拖带电机
- LY_CTHL_bFKCXTDDW_Step = 5;
- LY_CTHL_XSavePosBuff = dwXRealPos;
- SetPos(X_AXIS, 0); //启动位置设为0点
- dwXRealPos = 0;
- }
- break;
- case 5:
- if(LY_CTHL_XKSC_Limit_IN) //方块到位
- {
- AxisEgmStop(X_AXIS);
- LY_CTHL_QDXK_VAVLE=1;
- LY_CTHL_bFKCXTDDW_Step = 6;
- LY_CTHL_XKTD_VAVLE = 0; //斜口推带气缸关闭
- LY_CTHL_bFKCXTDDW_Delay = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME;
-
- }
- else if(dwXRealPos >= (LY_CTHL_XSavePosBuff + 800))
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_FKCXDW_ALARM,bRunning);//方块插销定位异常警告
-
- }
- break;
- case 6:
- if(LY_CTHL_PARAM_TDFZ_LENTH && !X_DRV) //拖带电机反转
- AxisMovePosAccDec(X_AXIS,8000,-LY_CTHL_PARAM_DXKFZ_LENTH,800,600,6,6,0);
- if(LY_CTHL_PARAM_CXLY_MODE)
- LY_CTHL_CXLY_VAVLE = 1;
- LY_CTHL_bFKCXTDDW_Step = 0;
- break;
-
- }
- }
- //插销穿入动作
- void LY_CTHL_bCXCRStep(void)
- {
- switch(LY_CTHL_bCXCR_Step)
- {
- case 11:
- LY_CTHL_XKTD_VAVLE = 1; //斜口推带气缸输出
- LY_CTHL_FKGD_VAVLE = 0; //方块固定关
- LY_CTHL_CXSCDW_VAVLE=1;
- LY_CTHL_bCXCR_Step = 12;
-
- break;
- case 12:
- if(!LY_CTHL_XKTD_Origin_IN) //斜口推带原位离开
- {
- LY_CTHL_bCXCR_Step = 13;
- // LY_CTHL_CXQJD_VAVLE = 1; //插销前夹带
- LY_CTHL_bCXCR_Delay = dwTickCount + 20;
- }
- break;
- case 13:
- if((LY_CTHL_CL_MotorStep == 0) && (LY_CTHL_SF_Origin_IN) && (dwTickCount >= LY_CTHL_bCXCR_Delay)) //插销前夹带回到原位
- {
- LY_CTHL_bCXCR_Step = 14;
- // LY_CTHL_FKGD_VAVLE = 1; //方块固定开
- LY_CTHL_CXQJD_VAVLE = 1; //插销前夹带
- LY_CTHL_bCXCR_Delay = dwTickCount + LY_CTHL_PARAM_XCQJD_XCDW_DELAY+100;
- }
- break;
- case 14:
- if(dwTickCount >= LY_CTHL_bCXCR_Delay)
- {
- LY_CTHL_bCXCR_Step = 15;
- LY_CTHL_FKGD_VAVLE = 1; //方块固定
- //Y轴启动插销
- LY_CTHL_CL_MotorStep = 20; //插销定位
- }
- break;
- case 15:
- if(LY_CTHL_CL_MotorStep == 0) //插销定位完成
- {
- LY_CTHL_bCXCR_Step = 16;
- LY_CTHL_CXDWZ_VAVLE = 1; //插销定位针(到位)电磁阀
- LY_CTHL_bCXCR_Delay = dwTickCount + 10; //延时插销下移
- }
- break;
- case 16:
- if(dwTickCount >= LY_CTHL_bCXCR_Delay)
- {
- LY_CTHL_bCXCR_Step = 17;
- if(LY_CTHL_PARAM_CXLY_MODE)
- LY_CTHL_CXLY_VAVLE = 0;
- else
- LY_CTHL_CXLY_VAVLE = 1; //Y06 插销下移电磁阀
- if(LY_CTHL_PARAM_NLSL_MODE == 0)
- LY_CTHL_bCXCR_Delay = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME; //插销下移异常时间
- else
- LY_CTHL_bCXCR_Delay = dwTickCount + 80; //插销下移异常时间
- }
- break;
- case 17:
- if((LY_CTHL_CXLY_Limit_IN) || LY_CTHL_PARAM_NLSL_MODE)
- {
-
- if(LY_CTHL_CXLY_Limit_IN || (dwTickCount >= LY_CTHL_bCXCR_Delay))
- {
- LY_CTHL_bCXCR_Step = 18;
- if( cXcxcrCnt==0 && !X_DRV) //拖带电机反转
- {
- AxisMovePosAccDec(X_AXIS,10000,-LY_CTHL_PARAM_TDFZ_LENTH,800,600,6,6,0);
- }
- LY_CTHL_CXQJD_VAVLE = 0; //插销前夹带电磁阀
- LY_CTHL_CXSCDW_VAVLE = 0; //Y21 插销上止定位关
- LY_CTHL_CXHJD_VAVLE = 1; //Y14 插销后夹带输出
- LY_CTHL_bCXCR_Delay = dwTickCount + LY_CTHL_PARAM_SCDWOff_CXDW_DELAY;
- }
- }
- else if(dwTickCount >= LY_CTHL_bCXCR_Delay)
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_CXLY_Limit_ALARM,bRunning);//插销下移异常
-
- }
- break;
- case 18:
- // if(START_IN_UP)
- {
- if(dwTickCount >= LY_CTHL_bCXCR_Delay)
- {
- LY_CTHL_bCXCR_Step = 19;
-
- LY_CTHL_CXHDW_VAVLE = 1; //插销后定位(插销穿入)
- LY_CTHL_bCXCR_Delay = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME +1000; //插销穿入到位异常时间
- LY_CTHL_CL_MotorStep = 50; //合链电机回原点
- }
- }
- break;
- case 19:
- if( cXcxcrCnt > 1)
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_CX_Limit_ALARM,bRunning); //插销穿入到位异常
- }
-
- if( LY_CTHL_CX_Limit_IN && cXcxcrCnt<2 )
- {
- cXcxcrCnt=0;
- bZhuangLiaoOkFlg = 0;
- LY_CTHL_bCXCR_Step = 0;
- LY_CTHL_CXDWZ_VAVLE = 0; //插销定位针(到位)电磁阀
-
- }
- else if (dwTickCount >= LY_CTHL_bCXCR_Delay)
- {
- cXcxcrCnt++;
-
- LY_CTHL_CXHJD_VAVLE=0;
- LY_CTHL_CXHDW_VAVLE=0;
- LY_CTHL_CXDWZ_VAVLE = 0;
- if(LY_CTHL_PARAM_CXLY_MODE)
- LY_CTHL_CXLY_VAVLE = 0;
- else
- LY_CTHL_CXLY_VAVLE = 1; //Y06 插销下移电磁阀
- LY_CTHL_bCXCR_Step = 11;
- }
-
- break;
- }
- }
- void LY_CTHL_bCLFirstStep(void)
- {
- switch(LY_CTHL_bCLFirst_Step)
- {
- case 19:
- LY_CTHL_JLTou_VAVLE = 0; //接拉头
- bZhuangLiaoOkFlg = 0;
- LY_CTHL_bCLFirst_Step = 20;
- LY_CTHL_CXDWZ_VAVLE = 0; //插销定位针(到位)电磁阀
- LY_CTHL_CXLY_VAVLE = 0; //Y06 插销下移电磁阀关
- LY_CTHL_bCLFirst_Delay = dwTickCount + LY_CTHL_PARAM_First_CL_DELAY;//延时一次穿入
- break;
- case 20:
- if(dwTickCount >= LY_CTHL_bCLFirst_Delay)
- {
- LY_CTHL_bCLFirst_Step = 21;
- LY_CTHL_First_CT_VAVLE = 1; //一次穿入电磁阀输出
- LY_CTHL_bCLFirst_Delay = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME;
- }
- break;
- case 21:
- if(!LY_CTHL_First_CT_Origin_IN) //原点离开后才开始时间
- {
- LY_CTHL_bCLFirst_Step = 22;
- LY_CTHL_bCLFirst_Delay = dwTickCount + LY_CTHL_PARAM_First_CL_Timer; //一次穿入时间
- }
- else if(dwTickCount >= LY_CTHL_bCLFirst_Delay)
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_YCCRYWYC_ALARM,bRunning); //一次穿入原位异常
- }
- break;
- case 22:
- if(dwTickCount >= LY_CTHL_bCLFirst_Delay)
- {
- LY_CTHL_bCLFirst_Step = 23;
- LY_CTHL_MGuo_VAVLE = 0; //码勾退
- LY_CTHL_bCLFirst_Delay = dwTickCount + LY_CTHL_PARAM_MGOff_XiaMo_DELAY;
- LY_CTHL_FKGD_VAVLE = 0; //方块固定退
- LY_CTHL_CXHJD_VAVLE = 0; //Y14 插销后夹带打开
- LY_CTHL_CXLY_VAVLE = 0; //Y06 插销下移电磁阀关
- }
- break;
- case 23:
- if(dwTickCount >= LY_CTHL_bCLFirst_Delay)
- {
- LY_CTHL_bCLFirst_Step = 24;
- LY_CTHL_CTXM_VAVLE = 0; //穿头下模下降
- LY_CTHL_CXHDW_VAVLE = 0; //插销后定位(插销穿入)关闭
- LY_CTHL_bCLFirst_Delay = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME;
- }
- break;
- case 24:
- if(!LY_CTHL_CTM_Limit_IN)
- {
- LY_CTHL_bCLFirst_Step= 25;
- LY_CTHL_bCLFirst_Delay = dwTickCount + 25; //一次穿入延时返回
- }
- else if(dwTickCount >= LY_CTHL_bCLFirst_Delay)
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_CTM_Origin_ALARM,bRunning); //穿头下模下降异常
- }
- break;
- case 25:
- if(dwTickCount >= LY_CTHL_bCLFirst_Delay)
- {
-
- LY_CTHL_bCLFirst_Step= 0;
- LY_CTHL_First_CT_VAVLE = 0; //一次穿入电磁阀关
- LY_CTHL_CXHDW_VAVLE = 0; //插销后定位(插销穿入)关闭
-
-
- //退下出错警告
- LY_CTHL_AutoDelay1 = dwTickCount + LY_CTHL_VAVLE_ERROR_TIME;
- LY_CTHL_AutoStep1 = 1;
- bTuiLaTouOkFlg = 1;
- }
- break;
- }
- }
- //手动动作
- void LY_CTHL_ManualAction(void)
- {
- if(bRunning) // 运行灯输出
- {
- LY_CTHL_Run_State=1;
- LY_CTHL_Stop_State=0;
- }
- else
- {
- LY_CTHL_Stop_State=1;
- LY_CTHL_Run_State=0;
- }
-
- if((LY_CTHL_SF_ALR_IN && (LY_CTHL_PARAM_DB_MODE == 1))|| (((GetAlarm(Y_AXIS)==0) || (GetAlarm(X_AXIS)==0)) && (LY_CTHL_PARAM_DB_MODE == 0))) // 伺服报警
- {
- cCHULIANBAOJIN=1;
- LY_CTHL_SetAlarmCode(LY_CTHL_SF_ALR_ALARM,0);
- }
- if(dwTickCount >= LY_CTHL_bSFBBAOJIN_Delay)
- {
- LY_CTHL_SF_ALR_CLR=0;
- SetClr(Y_AXIS,MOTOR_ALARM);
- SetClr(X_AXIS,MOTOR_ALARM);
- }
-
- if(bRunning == 0)
- {
- if(bClearTotal) //切断计数清零
- {
- bClearTotal = 0;
- ClrcToTal(QDCT_TOTAL_ADDR);
- }
- if(LY_CTHL_bTLiao ) //手动推料(推拉头)
- {
- LY_CTHL_bTLiao = 0;
- if(LY_CTHL_TLiao_VAVLE)
- LY_CTHL_TLiao_VAVLE = 0;//LY_CTHL_TLiao_VAVLE;
- else if(!LY_CTHL_HSLiao_VAVLE && LY_CTHL_HSLiao_Origin_IN)
- {
- LY_CTHL_TLiao_VAVLE = 1;
- }
- else//条件警告
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_HENSONGLIAO_ALARM,0); // 横送料不能输出
- }
- }
- if(LY_CTHL_bHSL) //手动横送料
- {
- LY_CTHL_bHSL= 0;
- if(!LY_CTHL_First_CT_VAVLE)
- {
- if(LY_CTHL_HSLiao_VAVLE)
- {
- LY_CTHL_HSLiao_VAVLE = 0;//~LY_CTHL_HSLiao_VAVLE;
- }
- else if(!LY_CTHL_TLiao_VAVLE && !LY_CTHL_TLiao_Limit_IN && LY_CTHL_CTM_Origin_IN && !LY_CTHL_JLiao_Limit_IN &&
- !LY_CTHL_JLTou_VAVLE && !LY_CTHL_CTXM_VAVLE && !LY_CTHL_CXDWZ_VAVLE && (!LY_CTHL_bMG || (LY_CTHL_bMG && LY_CTHL_LTou_Check)))
- {
- LY_CTHL_HSLiao_VAVLE = 1;
- }
- else //条件警告 最后一个括号条件为已经有拉头
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_YCCT_ALARM,0);
- }
- }
- }
- if(LY_CTHL_bJLT) //接拉头
- {
- LY_CTHL_bJLT = 0;
- if(LY_CTHL_JLTou_VAVLE)
- LY_CTHL_JLTou_VAVLE = 0;
- else if(!LY_CTHL_First_CT_VAVLE && !LY_CTHL_CTXM_VAVLE && LY_CTHL_CTM_Origin_IN && LY_CTHL_First_CT_Origin_IN)
- {
- LY_CTHL_MGuo_VAVLE = 0;
- LY_CTHL_JLTou_VAVLE = 1;
- }
- else//条件警告
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_YCXM_Origin_ALARM,0);
- }
- }
- if(LY_CTHL_bMG) //码勾
- {
- LY_CTHL_bMG = 0;
- LY_CTHL_MGuo_VAVLE = !LY_CTHL_MGuo_VAVLE;
- }
- if(LY_CTHL_bFKJD) //方块夹带
- {
- LY_CTHL_bFKJD = 0;
- LY_CTHL_FKJD_VAVLE = !LY_CTHL_FKJD_VAVLE;
- }
- if(LY_CTHL_bXKTD) //斜口推带
- {
- LY_CTHL_bXKTD = 0;
- if(LY_CTHL_XKTD_VAVLE)
- LY_CTHL_XKTD_VAVLE = 0;
- else if(!LY_CTHL_DXK_VAVLE && (!LY_CTHL_XKSC_Limit_IN || !LY_CTHL_CTXM_VAVLE|| LY_CTHL_PARAM_NLSL_MODE))
- LY_CTHL_XKTD_VAVLE = 1;
- else//条件警告
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_DXKCTXM_Origin_ALARM,0);
- }
- }
- if(LY_CTHL_bXKDW) //斜口定位
- {
- LY_CTHL_bXKDW = 0;
- LY_CTHL_XKDW_VAVLE = ~LY_CTHL_XKDW_VAVLE;
- }
- if(LY_CTHL_bCTXM) //穿头下模
- {
- LY_CTHL_bCTXM = 0;
- if(LY_CTHL_CTXM_VAVLE )
- {
- if(!LY_CTHL_CXLY_VAVLE)
- {
- LY_CTHL_CTXM_VAVLE = 0;
- LY_CTHL_JLTou_VAVLE = 0;
- }
- else
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_TXC_Limit_ALARM,0);
- }
- }
- else if(!LY_CTHL_HSLiao_VAVLE && LY_CTHL_HSLiao_Origin_IN && (!LY_CTHL_XKSC_Limit_IN || !LY_CTHL_XKTD_VAVLE))
- {
- if(LY_CTHL_PARAM_CXLY_MODE)
- LY_CTHL_CXLY_VAVLE = 1;
- LY_CTHL_MGuo_VAVLE = 1;
- LY_CTHL_CTXM_VAVLE = 1;
- }
- }
- if(LY_CTHL_bQianDXK) //前顶斜口
- {
- LY_CTHL_bQianDXK = 0;
- if(LY_CTHL_QDXK_VAVLE)
- LY_CTHL_QDXK_VAVLE = 0;
- else if(!LY_CTHL_XKTD_VAVLE)
- LY_CTHL_QDXK_VAVLE = 1;
- else //条件警告
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_XKTD_ALARM,0);
- }
- }
-
- if(LY_CTHL_bDXK) //顶斜口
- {
- LY_CTHL_bDXK = 0;
- if(!LY_CTHL_PARAM_NLSL_MODE)
- {
- if(LY_CTHL_DXK_VAVLE)
- LY_CTHL_DXK_VAVLE = 0;
- else if(!LY_CTHL_XKTD_VAVLE)
- {
- if(!LY_CTHL_PARAM_NLSL_MODE)
- LY_CTHL_DXK_VAVLE = 1;
- }
- else //条件警告
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_XKTD_ALARM,0);
- }
- }
- else
- {
- if(LY_CTHL_XKTD_VAVLE)
- LY_CTHL_XKTD_VAVLE = 0;
- else if(!LY_CTHL_DXK_VAVLE )
- {
- if(!LY_CTHL_XKSC_Limit_IN || !LY_CTHL_CTXM_VAVLE || LY_CTHL_PARAM_NLSL_MODE)
- LY_CTHL_XKTD_VAVLE = 1;
- else
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_YCXM_Origin_ALARM,0);
- }
- }
- else//条件警告
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_DXKCTXM_Origin_ALARM,0);
- }
- }
- }
-
- if(LY_CTHL_bQianDXK_DXK)
- {
- LY_CTHL_bQianDXK_DXK = 0;
- if(!LY_CTHL_XKTD_VAVLE && LY_CTHL_AutoStep==0)
- {
- cQDXK_DXK=1;
- LY_CTHL_AutoStep = 8;
- SetEn(X_AXIS, MOTOR_EN);
- LY_CTHL_AutoDelay = dwTickCount + 50;
- bRunning=1;
- }
-
- }
- //斜码插入
- if(LY_CTHL_bBanZiDong)
- {
- LY_CTHL_bBanZiDong=0;
- cBanAuTo=1;
- // LY_CTHL_PARAM_BanAuto_MODE=1;
- LY_CTHL_ZhuangLiaoDelay = dwTickCount + 150;
- LY_CTHL_MGuo_VAVLE = 1; //判断是否已经有料
- LY_CTHL_ZhuangLiaoStep = 1;
- bTuiLaTouOkFlg = 1;
- bZhuangLiaoOkFlg= 0;
- bCL_EN_FLG=0;
- LY_CTHL_AutoStep = 1;
- bRunning=1;
- }
- //小插插入
- if(LY_CTHL_CXC)//穿小插
- {
- LY_CTHL_CXC=0;
- cCXCONE=1;
- cBanAuTo=0;
- // LY_CTHL_PARAM_BanAuto_MODE=0;
- LY_CTHL_AutoStep = 11;
- bRunning=1;
- if(GetEn(Y_AXIS) == MOTOR_DISEN)
- {
- SetEn(Y_AXIS, MOTOR_EN);
- LY_CTHL_CL_MotorDelay = dwTickCount + 260; //锁轴时间
-
- }
- }
-
- if(LY_CTHL_bFKGD) //方块固定
- {
- LY_CTHL_bFKGD = 0;
- LY_CTHL_FKGD_VAVLE = ~LY_CTHL_FKGD_VAVLE;
- }
- if(LY_CTHL_bCXQJD) //插销前夹带
- {
- LY_CTHL_bCXQJD = 0;
- LY_CTHL_CXQJD_VAVLE = ~LY_CTHL_CXQJD_VAVLE;
- }
- if(LY_CTHL_bCXSY) //插销下移电磁阀
- {
- LY_CTHL_bCXSY = 0;
- LY_CTHL_CXLY_VAVLE = !LY_CTHL_CXLY_VAVLE;
- }
- if(LY_CTHL_bCXDWZ) //Y14 插销定位针电磁阀
- {
- LY_CTHL_bCXDWZ = 0;
- if(LY_CTHL_CXDWZ_VAVLE)
- LY_CTHL_CXDWZ_VAVLE = 0;
- else if(LY_CTHL_HSLiao_Origin_IN)
- {
- LY_CTHL_CXDWZ_VAVLE = 1;
- }
- else //条件警告
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_HENSONGLIAO_ALARM,0);
- }
-
- }
- if(LY_CTHL_bCXHJD) //Y15 插销后夹带电磁阀
- {
- LY_CTHL_bCXHJD = 0;
- LY_CTHL_CXHJD_VAVLE =~LY_CTHL_CXHJD_VAVLE;
- }
- if(LY_CTHL_bCXHDW) //Y16 插销后定位电磁阀
- {
- LY_CTHL_bCXHDW = 0;
- if(LY_CTHL_CXHDW_VAVLE)
- LY_CTHL_CXHDW_VAVLE = 0;
- else if(!LY_CTHL_CXQJD_VAVLE)
- LY_CTHL_CXHDW_VAVLE = 1;
- else//条件警告
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_CXQJD_ALARM,0);
- }
- }
- if(LY_CTHL_bFirst_CT) //Y20 一次穿头电磁阀
- {
- LY_CTHL_bFirst_CT = 0;
- if( !LY_CTHL_First_CT_VAVLE )
- {
- if(!LY_CTHL_CXLY_VAVLE)
- LY_CTHL_First_CT_VAVLE=1;
- else
- LY_CTHL_SetAlarmCode(LY_CTHL_TXC_Limit_ALARM,0);
- }
- else if(LY_CTHL_First_CT_VAVLE)
- {
- LY_CTHL_First_CT_VAVLE=0;
- }
-
- }
- if(LY_CTHL_bHLJJ) //Y21 合链夹具电磁阀
- {
- LY_CTHL_bHLJJ = 0;
- LY_CTHL_HLJJ_VAVLE = ~LY_CTHL_HLJJ_VAVLE;
- }
- if(LY_CTHL_bCXSCDW) //Y23 插销上止定位电磁阀
- {
- LY_CTHL_bCXSCDW = 0;
- LY_CTHL_CXSCDW_VAVLE = ~LY_CTHL_CXSCDW_VAVLE;
- }
- //自动装料
- if(LY_CTHL_bAutoZhuangLiao)
- {
- LY_CTHL_bAutoZhuangLiao = 0;
- if(LY_CTHL_ZhuangLiaoStep == 0)
- {
- bTuiLaTouOkFlg = 1;
- bZhuangLiaoOkFlg = 0;
- LY_CTHL_ZhuangLiaoStep = 1;
- LY_CTHL_MGuo_VAVLE = 1; //判断是否已经有料
- LY_CTHL_ZhuangLiaoDelay = dwTickCount + 150;
- }
- }
- //测试电机
- if(LY_CTHL_bCLMotor_N)
- {
- SetEn(Y_AXIS, MOTOR_EN);
- if(!LY_CTHL_SF_Origin_IN)
- {
- if(!Y_DRV)
- // Y轴 运行速度 启动速度 加速度 减速度
- AxisContinueMoveAcc(Y_AXIS,2000,DIR_N,1000,1000,15,15);
- }
- else
- {
- LY_CTHL_bCLMotor_N = 0;
- }
-
- }
-
- if(LY_CTHL_bCLMotor_P) //后退限位已经取消
- {
- SetEn(Y_AXIS, MOTOR_EN);
- if(!Y_DRV)
- // Y轴 运行速度 启动速度 加速度 减速度
- AxisContinueMoveAcc(Y_AXIS,2000,DIR_P,1000,1000,15,15);
- }
-
- if(!LY_CTHL_bCLMotor_P && !LY_CTHL_bCLMotor_N && (LY_CTHL_CL_MotorStep == 0))
- {
- AxisEgmStop(Y_AXIS);//AxisDecStop(Y_AXIS);
- }
- //合链电机返回原点
- if(LY_CTHL_bCLMotor_O)
- {
- LY_CTHL_bCLMotor_O = 0;
- if(LY_CTHL_CL_MotorStep == 0)
- LY_CTHL_CL_MotorStep = 40;
- }
- //拖带方块定位 ,因这步为前拖带定位, 很多电磁阀要关掉
- if(LY_CTHL_bFKTDDW)
- {
- LY_CTHL_bFKTDDW = 0;
- if(LY_CTHL_TD_MotorStep == 0)
- {
- LY_CTHL_XKDW_VAVLE = 0; //斜口定位
- LY_CTHL_CXQJD_VAVLE = 0; //插销前夹带
- if(!LY_CTHL_PARAM_CXLY_MODE)
- LY_CTHL_CXLY_VAVLE = 0; //插销下移
-
- LY_CTHL_CXHJD_VAVLE = 0; //插销后夹带
- LY_CTHL_CXHDW_VAVLE = 0; //插销后定位
- LY_CTHL_CXDWZ_VAVLE = 0; //插销定位针
- LY_CTHL_bHLJJ = 0; //合链夹具
- LY_CTHL_FKJD_VAVLE = 0; //方块夹带
- if((GetEn(X_AXIS) == MOTOR_DISEN))
- {
- SetEn(X_AXIS, MOTOR_EN);
- LY_CTHL_TD_MotorDelay = dwTickCount + 200;
- }
- else
- LY_CTHL_TD_MotorDelay = dwTickCount + 50;
- LY_CTHL_TD_MotorStep = 1;
- }
- }
- //拖带方块插销定位(斜口推带,斜口定位,顶斜口)
- if(LY_CTHL_bFKCXTDDW)
- {
- LY_CTHL_bFKCXTDDW = 0;
- if((LY_CTHL_bFKCXTDDW_Step == 0) && (LY_CTHL_CL_MotorStep == 0) &&
- !LY_CTHL_CTXM_VAVLE)
- {
- LY_CTHL_bFKCXTDDW_Step = 1;
- SetEn(X_AXIS, MOTOR_EN);
- LY_CTHL_CXQJD_VAVLE = 0; //插销前夹带
- LY_CTHL_CXLY_VAVLE = 0; //插销下移
- LY_CTHL_CXHJD_VAVLE = 0; //插销后夹带
- LY_CTHL_CXHDW_VAVLE = 0; //插销后定位
- LY_CTHL_CXDWZ_VAVLE = 0; //插销定位针
- LY_CTHL_HLJJ_VAVLE = 0; //合链夹具
- }
- }
- //插销穿入(包含电机前定位,插销下移,后夹,后定位)
- if(LY_CTHL_bCXCR)
- {
- LY_CTHL_bCXCR = 0;
- //拉头模在上面到们并且码勾输出,判断有拉头
- if(LY_CTHL_CTXM_VAVLE && LY_CTHL_MGuo_VAVLE && !LY_CTHL_CXQJD_VAVLE &&
- LY_CTHL_CTM_Limit_IN && !LY_CTHL_LTou_Check)
- {
- if(LY_CTHL_CL_MotorStep == 0)
- {
- LY_CTHL_bCXCR_Step = 11;
- SetEn(Y_AXIS, MOTOR_EN);
- if(!LY_CTHL_SF_Origin_IN)
- LY_CTHL_CL_MotorStep = 40;
- }
- }
- else //警告不能自动穿插销
- {
- LY_CTHL_SetAlarmCode(LY_CTHL_WFZIDONGCX_ALARM,0);
- }
- }
- //电机合链动作
- if(LY_CTHL_bMotorHL)
- {
- LY_CTHL_bMotorHL = 0;
- if(LY_CTHL_SF_Origin_IN)
- {
- if((LY_CTHL_CL_MotorStep == 0) && !LY_CTHL_CTXM_VAVLE && (!LY_CTHL_CTM_Limit_IN))
- {
- bCL_OK_FLG = 0;
- LY_CTHL_HLJJ_VAVLE = 1; //Y20 合链夹具电磁阀输出后,电机穿入
- LY_CTHL_CL_MotorStep = 1;
- LY_CTHL_CL_MotorDelay = dwTickCount + LY_CTHL_PARAM_HLJJ_CL_DELAY + 130;
- if((GetEn(Y_AXIS) == MOTOR_DISEN))
- {
- SetEn(Y_AXIS, MOTOR_EN);
- LY_CTHL_CL_MotorDelay = dwTickCount + 250;
- }
- }
- }
- else
- {
- LY_CTHL_bCLMotor_O =1;
-
- }
- }
- //一次穿链动作
- if(LY_CTHL_bCL_First)
- {
- LY_CTHL_bCL_First = 0;
- //拉头模在上面到们并且码勾输出,判断有拉头
- if(LY_CTHL_CTXM_VAVLE && LY_CTHL_MGuo_VAVLE && !LY_CTHL_CXQJD_VAVLE &&
- LY_CTHL_CTM_Limit_IN && !LY_CTHL_LTou_Check && (LY_CTHL_CL_MotorStep == 0))
- {
- LY_CTHL_bCLFirst_Step = 19;
- LY_CTHL_CXHJD_VAVLE = 1; //插销后夹带
- LY_CTHL_CXDWZ_VAVLE = 0; //插销定位针(到位)电磁阀
- LY_CTHL_CXQJD_VAVLE = 0; //插销前夹带
- }
- }
- }
- }
- #endif
|