123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872187318741875187618771878187918801881188218831884188518861887188818891890189118921893189418951896189718981899190019011902190319041905190619071908190919101911191219131914191519161917191819191920192119221923192419251926192719281929193019311932193319341935193619371938193919401941194219431944194519461947194819491950195119521953195419551956195719581959196019611962196319641965196619671968196919701971197219731974197519761977197819791980198119821983198419851986198719881989199019911992199319941995199619971998199920002001200220032004200520062007200820092010201120122013201420152016201720182019202020212022202320242025202620272028202920302031203220332034203520362037203820392040204120422043204420452046204720482049205020512052205320542055205620572058205920602061206220632064206520662067206820692070207120722073207420752076207720782079208020812082208320842085208620872088208920902091209220932094209520962097209820992100210121022103210421052106210721082109211021112112211321142115211621172118211921202121212221232124212521262127212821292130213121322133213421352136213721382139214021412142214321442145214621472148214921502151215221532154215521562157215821592160216121622163216421652166216721682169217021712172217321742175217621772178217921802181218221832184218521862187218821892190219121922193219421952196219721982199220022012202220322042205220622072208220922102211221222132214221522162217221822192220222122222223222422252226222722282229223022312232223322342235223622372238223922402241224222432244224522462247224822492250225122522253225422552256225722582259226022612262226322642265226622672268226922702271227222732274227522762277227822792280228122822283228422852286228722882289229022912292229322942295229622972298229923002301230223032304230523062307230823092310231123122313231423152316231723182319232023212322232323242325232623272328232923302331233223332334233523362337233823392340234123422343234423452346234723482349235023512352235323542355235623572358235923602361236223632364236523662367236823692370237123722373237423752376237723782379238023812382238323842385238623872388238923902391239223932394239523962397239823992400240124022403240424052406240724082409241024112412241324142415241624172418241924202421242224232424242524262427 |
- #include "global.h"
- #if JUN_YUE_MACHINE == 1
- unsigned long CTXM_DOWN_TIME=0,PRV_CTXM_DOWN_TIME=0;
- unsigned long HSLIAO_OUT_DELAY=0,PRV_HSLIAO_OUT_DELAY=0;
- unsigned char FZ_ALARM_FLG=0;
- unsigned short XCCR_LENGTH;
- unsigned short CTHL_XCCR_Time;//小插传入次数
- void CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag);
- void CTHL_ManualAction(void);
- void CTHL_AutoStepAction(void);
- void CTHL_TD_Motor(void);
- void CTHL_FZ_Motor(void);
- void CTHL_CheckStart(void);
- void CTHL_TLiao_Step(void); //推料(推拉头)
- void CTHL_AlarmProtect(void);
- void CTHL_ZLT_Step(void);
- void CTHL_ZhenDongAction(void);
- void CTHL_CTXMSS_Proc(void);
- void ExtiAcitionX02(void)
- {
-
- }
- //故障报警
- void CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag)
- {
- SetAlarmCode(CTHL_ALARM_ADDR,alarm_code);
- if(!CTHL_wltzi && !bCxcAuto )
- bAlarmStop = 1;
- if(bCxcAuto)
- {
- SetEn(X_AXIS, MOTOR_EN);
- SetEn(Y_AXIS, MOTOR_EN);
- }
-
- }
- //初始化动作
- void CTHL_InitAction(void)
- {
- float buff_pulse,buff_dist;
-
- buff_pulse = CTHL_PARAM_TDCYCLE_PULSE;
- buff_dist = CTHL_PARAM_TDCYCLE_LENGTH;
-
- XGearRatio = buff_pulse/buff_dist;
-
- buff_pulse = CTHL_PARAM_TDCYCLE_PULSE;
- buff_dist = CTHL_PARAM_FZCYCLE_LENGTH;
- YGearRatio = buff_pulse/buff_dist;
-
-
- //传递伺服齿轮比参数
-
- set_com_servo_param(X_AXIS,CTHL_PARAM_TDCYCLE_PULSE,CTHL_PARAM_TDCYCLE_LENGTH);
- set_com_servo_param(Y_AXIS,CTHL_PARAM_TDCYCLE_PULSE,CTHL_PARAM_FZCYCLE_LENGTH);
-
- SetAlarmCode(CTHL_ALARM_ADDR,0);
-
-
- 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, 0);
- SetDirReverse(Y_AXIS, 0);
- SetDirReverse(Z_AXIS, 0);
-
- //设置伺服告警极性,
- //伺服设置常开,则极性设置为0, 0为正常,1为告警,;
- //伺服设置为常闭,则极性设置为1,0,为正常,1位告警
- //SetAlarmReverse(X_AXIS,1);
- //SetAlarmReverse(Y_AXIS,0);
-
- SetServoComUse(X_AXIS,0);//Y轴正常模式。
- SetServoComUse(Y_AXIS,1);//X轴伺服扭矩通讯。
-
- SetEn(X_AXIS, MOTOR_EN);
- SetEn(Y_AXIS, MOTOR_EN);
- SetEn(Z_AXIS, MOTOR_EN);
- SetPos(X_AXIS, 0); //启动位置设为0点
- SetPos(Y_AXIS, 0); //启动位置设为0点
- SetPos(Z_AXIS, 0); //启动位置设为0点
-
- cBAOjin=1;
- }
- void CTHL_Action(void)
- {
- dwXRealPos = GetPos(X_AXIS);
- dwYRealPos = GetPos(Y_AXIS);
- dwZRealPos = GetPos(Z_AXIS);
- dwYRealPos_com=get_encode_value(Y_AXIS);
-
- user_datas[121] = CTHL_PARAM_XMZM_MODE;//XCCR_LENGTH;//CTXM_DOWN_TIME;//dwYRealPos_com;//servo_y.init_step;//CTHL_ZhuangLiaoStep;
- user_datas[122] = CTHL_CTXMSSStep;
- user_datas[123] = CTHL_TD_MotorStep;
- user_datas[124] = CTHL_LianLengthCheck;//CTHL_ZhuangLiaoStep;
- user_datas[125] = CTHL_LianLengthCheckOld;//CTHL_TLiaoStep;
- user_datas[126] = CTHL_AutoStep;//CTHL_FZ_MotorStep;//CTHL_TD_MotorStep;
- // user_datas[127] = CTHL_cZipCnt;//CTHL_ZhuangLiaoStep;
- // user_datas[128] = CTHL_FZ_MotorStep;//CTHL_LianLengthAutolengthcheck;
- CTHL_SERVO_COM=servo_y.IO_TO_COM;
-
- CTHL_ZhenDongAction();
- CTHL_AlarmProtect();
- CTHL_CheckStart();
- CTHL_ZLT_Step();
- CTHL_TLiao_Step();
- CTHL_TD_Motor();
- CTHL_FZ_Motor();
- CTHL_ManualAction();
- CTHL_AutoStepAction();
- CTHL_Test++;
- servo_com_run();
- CTHL_CTXMSS_Proc();
- }
- void CTHL_AlarmProtect(void)
- {
- }
- //自动动作
- void CTHL_AutoStepAction(void)
- {
- unsigned short freq;
- //测试用
- if(dwTickCount >= CTHL_TestDelay)
- {
- CTHL_TestDelay = dwTickCount + 200;
- CTHL_TestLED = !CTHL_TestLED;
- CTHL_Test = 0;
- }
- if(bRunning)
- {
- switch(CTHL_AutoStep)
- {
- case 1:
- if(dwTickCount >= CTHL_AutoDelay)
- {
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
-
- CTHL_TXM_VAVLE=0;
- CTHL_AutoStep = 2;
- //设置反转电机的扭矩和速度的设置
- freq=CTHL_PARAM_FZ_Speed+SERVO_PARAM_SPEED_DIFFER;
- if(CTHL_cZipCnt > 1)
- {
- set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMIN,freq,freq,SERVO_TARR_CW);
- }
- else
- {
- set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMAX,freq,freq,SERVO_TARR_CW);
- }
- Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
- bCxcAuto=0;
- if(CTHL_cZipCnt > 1) CTHL_AutoDelay = dwTickCount + 2;//拉头检测时间
- else CTHL_AutoDelay = dwTickCount + 100;//拉头检测时间
-
- if(CTHL_ZhuangLiaoStep == 0)
- {
- CTHL_ZhuangLiaoStep = 1;
- CTHL_TryCnt = 0;
- }
- CTHL_GF_Change = 0;
- }
- }
- break;
- case 2:
- if(bZhuangLiaoOkFlg && (CTHL_ZhuangLiaoStep == 0)&&(CTHL_TD_MotorStep==0)&&(CTHL_CTXMSSStep==0))
- {
- if(dwTickCount >= CTHL_AutoDelay )
- {
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
- if(!CTHL_PARAM_XMZM_MODE) {
- CTHL_QMDW_VAVLE = 1;//0为斜码,1为直码
- //CTHL_FKDW_VAVLE = 0; //方块定位退
- }
- CTHL_YXC_VAVLE=1;
- CTHL_YFK_VAVLE=1;
- CTHL_JXM_VAVLE = 1; //夹斜码电磁阀
- if(CTHL_cZipCnt==0)CTHL_CTXM_VAVLE=1;
- CTHL_JLTou_VAVLE = 0; //接拉头
- CTHL_AutoStep = 3;
- CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
- }
- }
- }
- break;
- case 3:
- if(CTHL_CTM_Limit_IN) //下模上到位
- {
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
- CTHL_AutoDelay = dwTickCount + CTHL_PARAM_XCDWFZ_DELAY;
- if(!CTHL_PARAM_XMZM_MODE)
- CTHL_AutoStep = 4;
- else
- CTHL_AutoStep = 5;
- }
- }
- else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_bDANBU_MODE)
- {
- CTHL_SetAlarmCode(CTHL_CTM_Limit_ALARM,bRunning);//横送料原位异常
- }
- break;
- case 4:
- if(CTHL_QMDW_IN&&dwTickCount >= CTHL_AutoDelay&&!X_DRV)
- {
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
- //AxisContinueMoveAcc(X_AXIS,1000,DIR_N,1000,600,15,5);
- AxisMovePosAccDec(X_AXIS,1000,-CTHL_PARAM_JMDW_LENGTH,1000,800,10,10,0); //前拖带松一点带
- AxisContinueMoveAcc(Y_AXIS,1000,DIR_N,1000,610,15,5);
-
- //Set_Ctrlmode_trans(Y_AXIS,TARR_MODE);
- CTHL_AutoStep = 5;
- CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
- CTHL_JXM_VAVLE = 1; //夹斜码电磁阀
- }
- }
- else if((dwTickCount >= CTHL_AutoDelay+3000)
- {
- CTHL_SetAlarmCode(CTHL_QMDW_Limit_ALARM,bRunning);//前码定位感应异常
- }
- break;
- case 5:
- if(!CTHL_QMDW_IN || CTHL_PARAM_XMZM_MODE)
- //if(!X_DRV|| CTHL_PARAM_XMZM_MODE)
- {
- //上升沿只能一次,不能做断点
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
- CTHL_AutoDelay = dwTickCount + CTHL_PARAM_TXM_DELAY;
- CTHL_AutoStep = 6;
- }
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- //Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
- }
- else if((dwTickCount >= CTHL_AutoDelay) && (!CTHL_bHandWork_MODE))
- {
- CTHL_SetAlarmCode(CTHL_QMDW_Limit_ALARM,bRunning);//前码定位感应异常
- }
- break;
- case 6:
- if(dwTickCount >= CTHL_AutoDelay) //顶斜码后拖反转
- {
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
- AxisMovePosAccDec(X_AXIS,2000,-CTHL_PARAM_XMDD_LENGTH,2000,800,10,10,0); //前拖带松一点带
- AxisMovePosAccDec(Y_AXIS,2000,20,2000,800,10,10,0);
- CTHL_TXM_VAVLE=1; //Y06推斜码电磁阀
- CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
- CTHL_AutoStep = 7;
- }
- }
- break;
- case 7:
- if(CTHL_TXM_Limit_IN && CTHL_TXM_VAVLE ) //推斜码到位
- {
- CTHL_AutoDelay = dwTickCount + CTHL_PARAM_TQMDW_DELAY;
- if(get_tarr_set(Y_AXIS)!=2)//伺服配置转矩限制失败
- {
- SetEn(Y_AXIS, MOTOR_DISEN);//设置扭矩做准备
- CTHL_wltzi=0;//停机
- CTHL_SetAlarmCode(CTHL_SERVO_CONFIG_ALARM,bRunning);//伺服转矩配置失败
- }
- CTHL_AutoStep = 8;
-
- }
- else if((dwTickCount >= CTHL_AutoDelay) && (!CTHL_bDANBU_MODE))
- {
- CTHL_SetAlarmCode(CTHL_TXM_Limit_ALARM,bRunning);//推斜码到异常
- }
- break;
- case 8:
- if(dwTickCount >= CTHL_AutoDelay&& !X_DRV&&!Y_DRV)
- {
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
- CTHL_QMDW_VAVLE=0;
- CTHL_FZ_MotorStep=1; //反转穿入
- //CTHL_FKDW_VAVLE = 0;
- //Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
- CTHL_FZ_MotorDelay = dwTickCount + CTHL_PARAM_CRTD_DELAY;//延时穿入拖带
- CTHL_AutoStep = 9;
- }
- }
- break;
- case 9:
- if(CTHL_FZ_MotorStep==0)
- {
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
- cBAOjin=0;
- CTHL_TFK_VAVLE=1;
- CTHL_YFK_VAVLE=0;
- CTHL_XCDW_VAVLE=1;
- CTHL_AutoStep = 10;
- CTHL_AutoDelay = dwTickCount + CTHL_PARAM_FKDW_FZSTOP_DELAY;//拖带定位
- }
- }
- break;
- case 10:
- if(dwTickCount >= CTHL_AutoDelay)
- {
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
- CTHL_FZ_MotorStep=20;
- CTHL_AutoStep = 11;
- CTHL_FZ_MotorDelay = dwTickCount + 0;
- }
- }
- break;
- case 11:
- if(CTHL_FZ_MotorStep==0 && !X_DRV && !Y_DRV)
- {
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
- CTHL_JCBI_VAVLE = 1;
- CTHL_SD = 1;
- CTHL_AutoStep = 12;
- CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
- }
- }
- break;
- case 12:
- if(CTHL_TXC_Limit_IN) //摆臂到位
- {
- //CTHL_DingDai_VAVLE=1; //暂时不用
- CTHL_XCCR_Time=0;
- //if(CTHL_PARAM_FZCXC_LENGTH != 0)
- // AxisMovePosAccDec(Y_AXIS,2*CTHL_PARAM_XCCR_SPEED,CTHL_PARAM_FZCXC_LENGTH,800,800,15,15,0);//辅助小插插入
- CTHL_AutoStep = 13;
- CTHL_AutoDelay = dwTickCount + CTHL_PARAM_XCIN_DELAY;//小插到位延时拖带
-
- }
- else if(dwTickCount >= CTHL_AutoDelay)
- {
- bCxcAuto=1;
- //CTHL_AutoStep = 101;
- //CTHL_JCBI_VAVLE = 0;
- //AxisMovePosAccDec(X_AXIS,1000,-50,1000,1000,15,15,0);//前轮布放松一点
- CTHL_SetAlarmCode(CTHL_XCBI_Limit_ALARM,bRunning);//小插摆臂到位异常
- }
- break;
- case 13:
- if(dwTickCount >= CTHL_AutoDelay && !Y_DRV)
- {
- SetPos(X_AXIS, 0);
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
- CTHL_AutoDelay = dwTickCount + 0;//CTHL_PARAM_YDC_DELAY;
- AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_XCCR_SPEED,CTHL_PARAM_XCCR_LENGTH,800,800,15,15,50);
- if(CTHL_SD)
- {
- CTHL_SD = 0;
- AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_XCCR_SPEED+100,CTHL_PARAM_XCCR_LENGTH+CTHL_PARAM_FZCXC_LENGTH,800,800,15,15,0);
- }
- CTHL_AutoStep = 14;
- }
- CTHL_XSavePosBuff3=dwXRealPos;
- }
- break;
- case 14:
- if(CTHL_XCDW_Limit_IN)//||(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_XCCRBH_LENGTH))
- {
- XCCR_LENGTH=dwXRealPos-CTHL_XSavePosBuff3;
- CTHL_XSavePosBuff3 = dwXRealPos;
- CTHL_AutoStep = 15;
-
- CTHL_XCDW_VAVLE=0;
- CTHL_AutoDelay = dwTickCount + CTHL_PARAM_TXCDW_DELAY;
- }
- //else if((dwTickCount >= (CTHL_AutoDelay+1000)) && (!CTHL_bHandWork_MODE))
- else if((dwTickCount >= (CTHL_AutoDelay+1200)||(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_XCCRBH_LENGTH)) && (!CTHL_bHandWork_MODE))
- {
- bCxcAuto=1;
- CTHL_XSavePosBuff3 = dwXRealPos;
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- CTHL_AutoStep = 100;
- CTHL_JCBI_VAVLE = 0; //Y16小插摆臂电磁阀
- //AxisMovePosAccDec(X_AXIS,800,-100,800,800,15,15,0);//前轮布放松一点
- CTHL_SetAlarmCode(CTHL_XCDW_Limit_ALARM,bRunning);//小插定位针异常
- }
- break;
- case 15:
- if((dwXRealPos >= (CTHL_XSavePosBuff3 + 50)))
- {
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- if(dwTickCount >= CTHL_AutoDelay)
- {
- CTHL_XCDW_VAVLE=0;
- CTHL_TFK_VAVLE = 0;
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
- cDELAYONE=0;
- bZhuangLiaoOkFlg=0;
- CTHL_AutoStep = 16;
- CTHL_AutoDelay = dwTickCount + 0;
- }
- }
- }
- break;
- case 16:
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
- if(dwTickCount >= CTHL_AutoDelay)
- {
- CTHL_AutoDelay = dwTickCount + 0;//CTHL_PARAM_TXCDW_DELAY ;
- bZhuangLiaoOkFlg=0;
- CTHL_AutoStep = 17;
- //AxisMovePosAccDec(X_AXIS,5000,-CTHL_PARAM_CRFZ_LENGTH,1500,800,10,10,0); //前拖带松一点带
- CTHL_YXC_VAVLE=0; //Y14压小插电磁阀
- CTHL_JCBI_VAVLE=0;
- }
- }
- break;
- case 100:
- if(CTHL_START_IN_UP)
- {
- SetEn(X_AXIS, MOTOR_DISEN);
- SetEn(Y_AXIS, MOTOR_DISEN);
- CTHL_AutoDelay = dwTickCount + 300 ;
- CTHL_AutoStep = 101;
- }
- break;
- case 101:
- if(dwTickCount >= CTHL_AutoDelay)
- {
- CTHL_XCCR_Time++;
- if(CTHL_XCCR_Time==1)
- {
- AxisMovePosAccDec(X_AXIS,800,-CTHL_XSavePosBuff3,800,800,15,15,0);//前轮布放松
- AxisMovePosAccDec(Y_AXIS,800,-CTHL_XSavePosBuff3,800,800,15,15,0);//前轮布放松
- }
- CTHL_AutoStep = 102;
- }
- break;
- case 102:
- if(!X_DRV&&!Y_DRV)
- {
- SetAlarmCode(CTHL_ALARM_ADDR,0);
- //AxisMovePosAccDec(X_AXIS,800,-100,800,800,15,15,0);//前轮布放松
- CTHL_YFK_VAVLE=1;
- CTHL_JCBI_VAVLE = 1; //Y16小插摆臂电磁阀
- // CTHL_XCDW_VAVLE=0;
- cDELAYONE=1;
- cBAOjin=1;
- CTHL_cZipCnt=0;
- bZhuangLiaoOkFlg=0;
- SetEn(X_AXIS, MOTOR_DISEN);
- SetEn(Y_AXIS, MOTOR_DISEN);
-
-
- // if(CTHL_FKGY_IN)
- // {
- // CTHL_AutoStep = 9;
- // CTHL_FZ_MotorStep==0;
- // }
- // else
- // {
- CTHL_AutoStep = 103;
- // }
- CTHL_AutoDelay = dwTickCount + 300 ;
- // if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE)
- // {
- SetClr(X_AXIS, MOTOR_NOALARM);
- SetClr(Y_AXIS, MOTOR_NOALARM);
- CTHL_bSFBBAOJIN_Delay= dwTickCount + 200;
- // }
- if(CTHL_PARAM_DBSF_MODE)
- {
- if(!CTHL_SF_ALR_IN)
- {
- CTHL_SF_ALR_CLR=1;
- CTHL_bSFBBAOJIN_Delay= dwTickCount + 200;
- }
- }
- }
- break;
- case 103:
- if(dwTickCount >= CTHL_AutoDelay)
- {
- CTHL_AutoStep = 104;
- CTHL_AutoDelay = dwTickCount + 100 ;
-
- }
- break;
- case 104:
- if(dwTickCount >= CTHL_AutoDelay && !Y_DRV)
- {
- CTHL_AutoDelay = dwTickCount + 0;//CTHL_PARAM_YDC_DELAY;
- AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_XCCR_SPEED,CTHL_PARAM_XCCR_LENGTH,800,800,15,15,50);
- CTHL_AutoStep = 105;
- CTHL_XSavePosBuff3=dwXRealPos;
- }
- break;
- case 105:
- if(CTHL_XCDW_Limit_IN)//||(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_XCCRBH_LENGTH))
- {
- XCCR_LENGTH=dwXRealPos-CTHL_XSavePosBuff3;
- CTHL_XSavePosBuff3 = dwXRealPos;
- CTHL_AutoStep = 15;
- CTHL_XCDW_VAVLE=0;
- CTHL_AutoDelay = dwTickCount + CTHL_PARAM_TXCDW_DELAY;
- }
- else if((dwTickCount >= (CTHL_AutoDelay+1200)||(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_XCCRBH_LENGTH+300)) && (!CTHL_bHandWork_MODE))
- {
-
- CTHL_XSavePosBuff3 = dwXRealPos;
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- if(CTHL_XCCR_Time>=2)
- {
- bCxcAuto=1;
- CTHL_SetAlarmCode(CTHL_XCDW_Limit_ALARM,bRunning);//小插定位针异常
- }
- else
- {
- CTHL_AutoStep = 100;
- CTHL_JCBI_VAVLE = 0; //Y16小插摆臂电磁阀
- }
- }
- break;
- case 17:
- if(dwTickCount >= CTHL_AutoDelay)
- {
- if(!X_DRV)
- {
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
- cDELAYONE=0;
- CTHL_TFK_VAVLE=0;
- CTHL_TD_MotorStep =10;
- CTHL_TD_MotorDelay = dwTickCount+ CTHL_PARAM_XCLD_DELAY;
- CTHL_AutoStep = 18;
- }
- }
- }
- break;
- case 18:
- //if(!CTHL_CTM_Limit_IN)
- {
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
- CTHL_JLTou_VAVLE = 0;
- //CTHL_CTXMSSStep=1;
- CTHL_AutoStep = 19;
-
- }
- }
- break;
- case 19:
- if((CTHL_TD_MotorStep == 0))
- {
- // if(CTHL_START_IN)
- {
- if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
- {
- CTHL_AutoStep = 1;
- if(CTHL_cStopMode==1 || bSingOneFlg)
- {
- CTHL_cStopMode=2;
- CTHL_ZhouWantwo=1;
- CTHL_CTXM_VAVLE =0;
- }
- AddToTal(CTHL_TOTAL_ADDR);
- AddToTal(CTHL_TRUST_ALARM_ADDR);
- CalProSP(CTHL_SPEED_ADDR);
- //公分节那条也不加
- if((CTHL_cZipCnt<3) && !CTHL_GF_Change)
- CTHL_cZipCnt++;
- if((CTHL_NoLaLian_IN))
- CTHL_cZipCnt = 0;
- if(bSingOneFlg || CTHL_bHandWork_MODE)
- {
- CTHL_AutoStep = 0;
- bRunning = 0;
- bSingOneFlg = 0;
- }
- CTHL_GF_Change = 0;
- }
- }
- }
- break;
-
- }
- }
- }
- //下模上升处理
- void CTHL_CTXMSS_Proc(void)
- {
- static unsigned char hemoflag1=0,hemoflag2=0;
- if(bRunning)
- {
- switch(CTHL_CTXMSSStep)
- {
- case 1:
- if(!CTHL_CTM_Limit_IN)
- {
- CTHL_JXM_VAVLE=0;
- PRV_CTXM_DOWN_TIME= dwTickCount;
- if(CTXM_DOWN_TIME)HSLIAO_OUT_DELAY=dwTickCount+CTXM_DOWN_TIME*7/8;
- hemoflag1=1;
- hemoflag2=1;
- CTHL_CTXMSSStep = 2;
- CTHL_AutoDelay1 = dwTickCount+ 3000;
- }
- break;
- case 2: //测量时间
- if(CTHL_CTM_Origin_IN&&hemoflag1)
- {
- hemoflag1=0;
- PRV_CTXM_DOWN_TIME=dwTickCount-PRV_CTXM_DOWN_TIME;
- CTXM_DOWN_TIME=PRV_CTXM_DOWN_TIME;
- }
- //
- /*if(CTXM_DOWN_TIME&&(CTHL_cZipCnt > 1))//测量时间后的动作
- {
- if(dwTickCount>=HSLIAO_OUT_DELAY&&hemoflag2)
- {
- hemoflag2=0;
- CTHL_ZhuangLiaoStep = 1;
- CTHL_TryCnt = 0;
- bZhuangLiaoOkFlg=0;
- CTHL_ZhuangLiaoDelay = dwTickCount;
- }
- if(CTHL_CTM_Origin_IN)
- {
- CTHL_CTXMSSStep = 0;
- CTHL_MGuo_VAVLE = 0;
- CTHL_JLTou_VAVLE = 0;
-
- }
- else if(dwTickCount >= CTHL_AutoDelay1)
- {
- //穿头模和一次穿入回位异常
- if(!CTHL_CTM_Origin_IN)
- CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,bRunning);
- }
- }
- else //测量时间前的动作*/
- {
- if(CTHL_CTM_Origin_IN)
- {
- CTHL_ZhuangLiaoStep = 1;
- CTHL_TryCnt = 0;
- bZhuangLiaoOkFlg=0;
- CTHL_ZhuangLiaoDelay = dwTickCount;
- CTHL_CTXMSSStep = 20;
- CTHL_MGuo_VAVLE = 0;
- CTHL_JLTou_VAVLE = 0;
- CTHL_GDLT_VAVLE=1;
-
- }
- else if(dwTickCount >= CTHL_AutoDelay1)
- {
- //穿头模和一次穿入回位异常
- if(!CTHL_CTM_Origin_IN)
- CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,bRunning);
- }
- }
- break;
- case 20:
- if(bZhuangLiaoOkFlg && (CTHL_ZhuangLiaoStep == 0)&&CTHL_CTXMSS_FLAG)
- {
- CTHL_CTXMSS_FLAG=0;
- if(!CTHL_LTou_Check && !CTHL_TXM_Limit_IN)//有拉头,推斜码到位不能亮
- {
- //if(!CTHL_ZhouWanone && !bSingOneFlg)
- {
- CTHL_CTXM_VAVLE=1;
- CTHL_CTXMSSStep = 21;
- }
- //else
- //{
- // CTHL_CTXMSSStep = 0;
- //}
- }
- else
- {
- CTHL_wltzi=1;
- CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning); // 没有拉头
- }
- }
- break;
- case 21:
- if(CTHL_CTM_Limit_IN)
- {
- CTHL_CTXMSSStep = 0;
- }
- break;
- }
- }
- }
- void CTHL_CheckStart(void)
- {
- //if(!CTHL_SafeDoor_IN && bRunning)
- //{
- // CTHL_SetAlarmCode(CTHL_SafeDoor_ALARM,0); // 安全门关上才能启动
- //}
- if(CTHL_START_IN_UP || bStart)
- {
- if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE)
- {
- SetClr(X_AXIS, MOTOR_NOALARM);
- SetClr(Y_AXIS, MOTOR_NOALARM);
- CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
- }
- if(CTHL_PARAM_DBSF_MODE)
- {
- if(!CTHL_SF_ALR_IN)
- {
- CTHL_SF_ALR_CLR=1;
- CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
- }
- }
- SetAlarmCode(CTHL_ALARM_ADDR,0);
- }
- if(CTHL_wltzi)//无拉头启动
- {
- if(CTHL_START_IN_UP || bStart)
- {
- if(bRunning)
- {
- CTHL_wltzi=0;
- CTHL_AutoStep = 1;
- CTHL_ZhuangLiaoStep = 1;
- bTuiLaTouOkFlg = 1;
- bZhuangLiaoOkFlg= 0;
- CTHL_MGuo_VAVLE = 1; //判断是否已经有料
- CTHL_GDLT_VAVLE=0;
- CTHL_ZhuangLiaoDelay = dwTickCount + 150;
- if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE)
- {
- SetClr(X_AXIS, MOTOR_NOALARM);
- SetClr(Y_AXIS, MOTOR_NOALARM);
- CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
- }
- if(CTHL_PARAM_DBSF_MODE)
- {
- if(!CTHL_SF_ALR_IN)
- {
- CTHL_SF_ALR_CLR=1;
- CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
-
- }
- }
- }
- else
- {
- CTHL_wltzi=0;
- }
- }
- }
-
- #if 1
- //启动 自动启动 单一自动
- if((CTHL_START_IN_UP || bStart || CTHL_bSingOne ) && !CTHL_wltzi)
- {
- if(!bRunning)
- {
- bRunning = 1;
- if( CTHL_bSingOne)bSingOneFlg=1;
- SetEn(X_AXIS, MOTOR_DISEN);
- SetEn(Y_AXIS, MOTOR_DISEN);
- CTHL_FZ_MotorStep = 0;
- CTHL_CTXMSSStep=0;
- CTHL_CTXMSS_FLAG=0;
-
- CTHL_PARAM_ZDP_AUTO=0;//振动盘
- CTHL_TryCnt = 0;
- bCxcAuto=0;
- bCL_EN_FLG = 0;
- CTHL_stop=0;
-
- CTHL_TLiao_VAVLE =0;
- CTHL_HSLiao_VAVLE =0;
- CTHL_JLTou_VAVLE =0;
- CTHL_CTXM_VAVLE =0;
- // CTHL_MGuo_VAVLE =0;
- CTHL_JXM_VAVLE =0;
- CTHL_TXM_VAVLE = 0 ;
- CTHL_QMDW_VAVLE =0;
- CTHL_TFK_VAVLE =0;
-
- CTHL_HL_VAVLE =0;
- CTHL_YFK_VAVLE =0;
- CTHL_YXC_VAVLE =0;
- CTHL_XCDW_VAVLE =0;
- CTHL_JCBI_VAVLE =0;
- CTHL_DingDai_VAVLE=0;
-
- CTHL_cZipCnt = 0;
- SetAlarmCode(CTHL_ALARM_ADDR,0);
- CTHL_AutoDelay = dwTickCount + 0;
- CTHL_TD_MotorStep = 0;
- CTHL_AutoStep = 1;
- CTHL_MGuo_VAVLE = 1; //判断是否已经有料
- CTHL_GDLT_VAVLE=0;
-
- //方块定位
- CTHL_TD_MotorStep=1;
-
- CTHL_ZhuangLiaoDelay = dwTickCount + 350;
- CTHL_ZhuangLiaoStep=1;
- bTuiLaTouOkFlg = 1;
- bZhuangLiaoOkFlg= 0;
-
- bFZ_LengthOK = 0;
- CTHL_LianLengthCheck = 0;
- }
- bStart = 0;
- CTHL_bSingOne=0;
- }
- if(CTHL_ZhouWantwo)//作为一条后停机
- {
- CTHL_ZhouWantwo=0;
- if(CTHL_ZhouWanone)
- {
- CTHL_ZhouWanone=0;
- bRunning=0;
- CTHL_cStopMode=0;
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE)
- {
- SetClr(X_AXIS, MOTOR_NOALARM);
- SetClr(Y_AXIS, MOTOR_NOALARM);
- CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
- }
- if(CTHL_PARAM_DBSF_MODE)
- {
- if(!CTHL_SF_ALR_IN)
- {
- CTHL_SF_ALR_CLR=1;
- CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
- }
- }
- SetEn(X_AXIS, MOTOR_EN);
- SetEn(Y_AXIS, MOTOR_EN);
-
- bSingOneFlg=0;
- CTHL_AutoStep = 0;
- CTHL_FZ_MotorStep =0;
- CTHL_TD_MotorStep =0;
- CTHL_CTXM_VAVLE =0;
- }
- }
- //停止
- if(CTHL_STOP_IN_UP || bStop)
- {
- if(CTHL_cStopMode < 2)CTHL_cStopMode++;
- if(CTHL_cStopMode==1 && bRunning)CTHL_ZhouWanone=1; //按一次停止
- if(bRunning && CTHL_cStopMode == 2||!bRunning)//按下2次停止
- {
-
- CTHL_cStopMode=0;
- bSingOneFlg=0;
- cBAOjin=0;
- CTHL_wltzi=0;
-
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
- SetEn(X_AXIS, MOTOR_EN);
- SetEn(Y_AXIS, MOTOR_EN);
-
- CTHL_AutoStep = 0;
- CTHL_stop=0;
- CTHL_FZ_MotorStep =0;
- CTHL_TD_MotorStep =0;
- if(!bRunning)
- {
- CTHL_ZhuangLiaoStep=0;
- CTHL_TLiaoStep=0;
- CTHL_TLiao_VAVLE=0;
- CTHL_HSLiao_VAVLE =0;
- CTHL_JLTou_VAVLE =0;
- }
- CTHL_TXM_VAVLE=0;
- CTHL_CTXM_VAVLE =0;
- CTHL_MGuo_VAVLE = 0;
- CTHL_JXM_VAVLE =0;
- CTHL_QMDW_VAVLE =0;
- CTHL_TFK_VAVLE =0;
-
- CTHL_HL_VAVLE =0;
- CTHL_YFK_VAVLE =0;
- CTHL_YXC_VAVLE =0;
- CTHL_XCDW_VAVLE=0;
- CTHL_JCBI_VAVLE =0;
- CTHL_DingDai_VAVLE=0;
-
- SetAlarmCode(CTHL_ALARM_ADDR,0);
- if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE)
- {
- SetClr(X_AXIS, MOTOR_NOALARM);
- SetClr(Y_AXIS, MOTOR_NOALARM);
- CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
- }
- if(CTHL_PARAM_DBSF_MODE)
- {
- if(!CTHL_SF_ALR_IN)
- {
- CTHL_SF_ALR_CLR=1;
- CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
- }
- }
- SetPos(X_AXIS, 0);
- SetPos(Y_AXIS, 0);
- SetPos(Z_AXIS, 0);
- SetAlarmCode(CTHL_ALARM_ADDR,0);
- bFZ_LengthOK = 0;
- CTHL_LianLengthCheck = 0;
- CTHL_LianLengthCheckOld = 0;
-
- bRunning=0;
- }
- bStop=0;
- }
- if(bAlarmStop)
- {
- bRunning = 0;
- bAlarmStop=0;
- CTHL_cStopMode=0;
- CTHL_XCDW_VAVLE=0;
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
- // AxisEgmStop(Z_AXIS);
- CTHL_AutoStep = 0;
- CTHL_FZ_MotorStep =0;
- CTHL_TD_MotorStep =0;
- }
- #endif
- }
- void CTHL_NoLaLian_Speed_Proc(void)
- {
- if(CTHL_NoLaLian_IN_UP)
- {
- AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,200,
- 1000,CTHL_PARAM_FKDWMAN_Speed/2,25,15,0); //YWX_CTHL_PARAM_TD_RunSpeed
- AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,DIR_P,1500,800,15,15);
- }
- }
- void CTHL_TDLength_BH_Proc(unsigned short local)//拖带长度保护
- {
- if(local==0)//为感应到开关前
- {
- if(CTHL_cZipCnt < 2)
- {
- if((dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length) && (!CTHL_bHandWork_MODE))
- {
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- user_datas[128]=CTHL_TD_MotorStep;
- CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机
- }
- }
- else
- {
- if((dwXRealPos >=(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheck)) && (!CTHL_bHandWork_MODE))
- {
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- user_datas[128]=CTHL_TD_MotorStep;
- CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机
- }
- }
- }
- else//感应到开关后
- {
- if(CTHL_cZipCnt < 2)
- {
- if((dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length+CTHL_LianLengthAutoFZCRcheck) && (!CTHL_bHandWork_MODE))
- {
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- user_datas[128]=CTHL_TD_MotorStep;
- CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机
- }
- }
- else
- {
- if((dwXRealPos >=(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheck+CTHL_LianLengthCheckOld)) && (!CTHL_bHandWork_MODE))
- {
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- user_datas[128]=CTHL_TD_MotorStep;
- CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机
- }
- }
- }
- }
- //拖带合链
- void CTHL_TDHL_Proc(void)
- {
- //合链
- if(dwXRealPos >= (CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1)&&!CTHL_bHandWork_MODE)
- {
- CTHL_XSavePosBuff2=dwXRealPos;
- CTHL_HL_VAVLE=1;
- }
- //合链结束
- if(CTHL_cZipCnt < 2)
- {
- if((dwXRealPos>(CTHL_LianLengthAutoFZCRcheck)))
- {
- CTHL_HL_VAVLE=0;
- }
- }
- else
- {
- if((dwXRealPos>(CTHL_PARAM_HLWZ_LENGTH+CTHL_LianLengthAutoFZCRcheck-CTHL_PARAM_HELIAN_LENGTH)))
- {
- CTHL_HL_VAVLE=0;
- }
- }
- }
- //无方块处理
- void CTHL_TDWFK_Proc(void)
- {
- }
- //拖带电机控制动作 X轴
- void CTHL_TD_Motor(void) //
- {
- if(dwTickCount >= CTHL_bSFBBAOJIN_Delay)
- {
- CTHL_SF_ALR_CLR=0;
- SetClr(X_AXIS,MOTOR_ALARM);
- SetClr(Y_AXIS,MOTOR_ALARM);
-
- }
- if(CTHL_PARAM_DBSF_MODE)
- {
- if(!CTHL_SF_ALR_IN)
- {
- bCxcAuto=1;
- CTHL_AutoStep = 201;
- CTHL_FZ_MotorStep =0;
- CTHL_XCDW_VAVLE=0;
- CTHL_TD_MotorStep =0;
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- CTHL_SetAlarmCode(CTHL_SF_ALR_ALARM,bRunning); //无拉链自动停机
- CTHL_JCBI_VAVLE = 0;
- }
- }
- if((GetAlarm(Y_AXIS)==1 || GetAlarm(X_AXIS)==1) && !CTHL_PARAM_DBSF_MODE && !cBAOjin)
- {
- bCxcAuto=1;
- cBAOjin=1;
- AxisEgmStop(X_AXIS);
- AxisEgmStop(Y_AXIS);
- CTHL_AutoStep = 201;
- CTHL_SetAlarmCode(CTHL_SF_ALR_ALARM,bRunning); //无拉链自动停机
- CTHL_JCBI_VAVLE = 0;
- // CTHL_TFK_VAVLE=0;
- }
- #if 1
- switch(CTHL_TD_MotorStep)
- {
- //1步开始,纯定位, 没有穿入
- case 1:
- if(dwTickCount >= CTHL_TD_MotorDelay) //给锁轴时间
- {
- //CTHL_FKDW_VAVLE=0;
- if(CTHL_PARAM_Test)
- CTHL_TXM_VAVLE = 0 ;
- else
- CTHL_TXM_VAVLE = 1; //因推斜码和固定方块一起,要避开
- CTHL_TD_MotorStep = 2;
- SetPos(X_AXIS, 0);
- AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_First_SPEED,DIR_P,1000,600,10,15);
- AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_First_SPEED * 4/5,DIR_P,1000,600,10,15);
- }
- break;
- case 2://检测到过链
- if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length)
- {
- CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机
- }
- if(CTHL_FK_Check_UP)
- {
- CTHL_XSavePosBuff = dwXRealPos;
- AxisContinueMoveChangeSpeed(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,1000,20,20);
- AxisContinueMoveChangeSpeed(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,1000,20,20);
- //AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_TDDW_LENGTH,
- // 1000,1000,30,100,50);
- //AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed*4/5,CTHL_PARAM_TDDW_LENGTH,
- // 1000,1000,30,100,50);
- CTHL_TD_MotorStep = 3;
- }
- break;
- case 3:
- if(dwXRealPos>=CTHL_XSavePosBuff+200)
- {
- //CTHL_FKDW_VAVLE=1;
- CTHL_TXM_VAVLE=0;
- CTHL_TD_MotorStep = 4;
- //CTHL_TD_MotorDelay = dwTickCount+ 1500;
- }
- break;
- case 4:
- //定位感应亮时、位置到达时,或X轴停止时,停止X轴
- //if(dwTickCount >= CTHL_TD_MotorDelay)//3S为检测到停机
- //{
- // bCxcAuto=1;
- // CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);//方块钩针定位异常
- //}
- if(CTHL_FKDW_IN_UP)
- {
- CTHL_TD_MotorStep = 5;
- CTHL_TD_MotorDelay = dwTickCount+ 0;
- AxisMovePosAccDec(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH,
- 1000,1000,30,100,50);
- AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH,
- 1000,1000,30,100,50);
- }
- break;
- case 5:
- if(!X_DRV&&!Y_DRV)
- {
- CTHL_TD_MotorStep = 0;
- }
- break;
- //穿合拉头定位
- case 10:
- if(dwTickCount >= CTHL_TD_MotorDelay)
- {
- CTHL_JCBI_VAVLE=0;
- CTHL_DingDai_VAVLE=0;
- CTHL_YXC_VAVLE=0;
- CTHL_YFK_VAVLE=0;
- CTHL_TXM_VAVLE=0;
- CTHL_TD_MotorStep = 11;
- SetPos(X_AXIS, 0); //启动位置设为0点
- if(CTHL_cZipCnt == 0)CTHL_LianLengthCheckOld = 0;
-
- AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_HLT_SPEED_Speed,CTHL_PARAM_HL_LENGTH,
- 1000,1000,40,25,00);
- if(!CTHL_bHandWork_MODE)//在手动穿链模式下不用动电机同步
- AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_HLT_SPEED_Speed,CTHL_PARAM_HL_LENGTH,
- 1000,1000,40,25,00);
-
- CTHL_XSavePosBuff = dwXRealPos;
- bFKCheck_Flag = 0;
- CTHL_XSavePosBuff3 = 0;
- }
- break;
- case 11:
- CTHL_NoLaLian_Speed_Proc(); //最后拉链慢速的处理
- //慢速合链长度,移动方块检测,躲避方块
- if(dwXRealPos >= CTHL_PARAM_HL_LENGTH+200)
- {
- if(CTHL_PARAM_Test)
- CTHL_TXM_VAVLE = 0 ;
- else
- CTHL_TXM_VAVLE = 1;
- }
- //第一方块信号检测
- if(CTHL_FK_Check_UP && !bFKCheck_Flag)
- {
- bFKCheck_Flag = 1;
- CTHL_XSavePosBuff3= dwXRealPos;
- }
- //慢速合链长度 如果已经到达下一条的方块也必须开模 ,CTHL_NoLaLian_IN最后几条慢速时感应才退下模
- if(((dwXRealPos >= CTHL_PARAM_HL_LENGTH) && (!CTHL_bHandWork_MODE) && !CTHL_NoLaLian_IN) || (bFKCheck_Flag))
- {
- //码勾先打开15MS再退下模
- if(CTHL_MGuo_VAVLE)
- {
- CTHL_MGuo_VAVLE=0;
- CTHL_GDLT_VAVLE=1;
- CTHL_TD_MotorDelay = dwTickCount + CTHL_PARAM_TMG_TXM_DELAY;;
- if(CTHL_NoLaLian_IN)
- CTHL_TD_MotorDelay = dwTickCount + 20;
- }
- else if(dwTickCount >= CTHL_TD_MotorDelay)
- {
- CTHL_XSavePosBuff1= dwXRealPos;
- CTHL_CTXM_VAVLE=0;//推下模
- CTHL_CTXMSSStep=1;
- CTHL_TD_MotorStep = 12;
- }
- }
- break;
- case 12://退下模加速
- CTHL_TDLength_BH_Proc(0);
- CTHL_NoLaLian_Speed_Proc();
- if(CTHL_FK_Check_UP && !bFKCheck_Flag)
- {
- bFKCheck_Flag = 1;
- CTHL_XSavePosBuff3= dwXRealPos;
- cGFJAUTO=0;
- }
- if(CTHL_CTM_Limit_IN)
- {
- CTHL_TD_MotorDelay = dwTickCount + CTHL_PARAM_TuiXiaMo_DELAY;
- }
- else if(dwTickCount >= CTHL_TD_MotorDelay) //下模离开后延时加速
- {
- if(!CTHL_bHandWork_MODE)
- {
- bHeLaTouOK = 1;
- if((CTHL_cZipCnt > 1) && (!CTHL_NoLaLian_IN))
- {
- if(CTHL_LianLengthCheckOld == 0)
- {
- CTHL_DataBuff = 6000;
- CTHL_LENTH=CTHL_LianLengthCheck - CTHL_PARAM_HL_LENGTH;
- }
- else if((CTHL_LianLengthCheck < 4000) && (CTHL_LianLengthCheckOld <4000))
- {
- CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED*3/4;
- CTHL_LENTH=CTHL_LianLengthCheck + CTHL_LianLengthCheckOld- dwXRealPos;
-
- }
- else if((CTHL_LianLengthCheck < 5000) && (CTHL_LianLengthCheckOld <5000))
- {
- CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED*4/5;
- CTHL_LENTH=CTHL_LianLengthCheck + CTHL_LianLengthCheckOld- dwXRealPos;
- }
- else
- {
- CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED;
- CTHL_LENTH=CTHL_LianLengthCheck + CTHL_LianLengthCheckOld- dwXRealPos;
- }
- AxisMovePosAccDecNotStop(X_AXIS,CTHL_DataBuff,CTHL_LENTH,
- 1000,CTHL_PARAM_FKDW_Speed,80,50,200);
- }
- else
- {
- if(CTHL_NoLaLian_IN)
- CTHL_DataBuff = CTHL_PARAM_FKDWMAN_Speed/2;
- else
- CTHL_DataBuff = CTHL_PARAM_First_SPEED;
- AxisContinueMoveAcc(X_AXIS,CTHL_DataBuff,DIR_P,1500,800,15,15);
- }
- AxisContinueMoveAcc(Y_AXIS,CTHL_DataBuff,DIR_P,1500,800,15,15);
- CTHL_TD_MotorStep = 13;
- }
- }
- break;
- case 13:
- CTHL_TDLength_BH_Proc(0);
- CTHL_NoLaLian_Speed_Proc();
- CTHL_TDHL_Proc();
- //已经开完模具
- if(CTHL_FK_Check_UP&& !bFKCheck_Flag)
- {
- bFKCheck_Flag = 1;
- CTHL_XSavePosBuff3= dwXRealPos;
- }
- else if(bFKCheck_Flag) //说明第一个方块已经检测到
- {
- CTHL_TD_MotorStep = 14;
- }
- break;
- case 14:
- CTHL_TDLength_BH_Proc(1);
- CTHL_NoLaLian_Speed_Proc();
- CTHL_TDHL_Proc();
- //无链运行15公分
- if(!CTHL_FK_Check && (dwXRealPos >= (CTHL_XSavePosBuff3 + 1500)))
- {
- CTHL_TD_MotorStep = 15;
- if(CTHL_cZipCnt > 1)CTHL_CTXMSS_FLAG=1;
- }
- break;
- case 15:
- CTHL_TDLength_BH_Proc(1);
- CTHL_NoLaLian_Speed_Proc();
- CTHL_TDHL_Proc();
-
- //第二次感应方块, 要做定位
- if(CTHL_FK_Check_UP && (dwXRealPos >= (CTHL_XSavePosBuff3 + 1800)))
- {
- if(!CTHL_bHandWork_MODE)
- {
- if(!CTHL_NoLaLian_IN)
- {
- CTHL_XSavePosBuff = dwXRealPos;
- AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,300,
- 1000,CTHL_PARAM_FKDWMAN_Speed,30,80,50);
- AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,300,
- 1000,CTHL_PARAM_FKDWMAN_Speed,30,80,50);
- }
- //长度测量
- bFZ_LengthOK = 1;
- if(CTHL_cZipCnt)
- {
- CTHL_LianLengthCheckOld = CTHL_LianLengthCheck;
- }
- CTHL_LianLengthCheck = dwXRealPos - CTHL_XSavePosBuff3;
- //公分节处理
- if((CTHL_LianLengthCheck <= CTHL_PARAM_GFJ_LENGTH) && (CTHL_cZipCnt>1))
- {
- CTHL_TD_MotorStep=50;
- AxisContinueMoveAcc(X_AXIS,2000,DIR_P,1000,600,15,15);
- AxisContinueMoveAcc(Y_AXIS,2000,DIR_P,1000,600,15,15);
- CTHL_TD_MotorDelay = dwTickCount + 200;
- bFZ_LengthOK= 0;
- CTHL_cZipCnt = 0;
- }
- else
- {
- CTHL_TD_MotorStep = 16;
- CTHL_AutoDelay=dwTickCount;
- if(CTHL_cZipCnt<2)CTHL_CTXMSS_FLAG=1;
- }
- }
- CTHL_XSavePosBuff = dwXRealPos;
- }
- CTHL_TDWFK_Proc();
- break;
- case 16:
- if(dwXRealPos>=CTHL_XSavePosBuff+200)
- {
- //CTHL_FKDW_VAVLE=1;
- CTHL_TD_MotorStep = 17;
- //CTHL_TD_MotorDelay = dwTickCount+ 1500;
- }
- break;
- case 17:
- //定位感应亮时、位置到达时,或X轴停止时,停止X轴
- //if(dwTickCount >= CTHL_TD_MotorDelay)//3S为检测到停机
- //{
- // bCxcAuto=1;
- // CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);//方块钩针定位异常
- //}
- if(CTHL_FKDW_IN)
- {
- if(CTHL_cZipCnt > 1&&!CTHL_NoLaLian_IN_UP)
- {
- AxisMovePosAccDec(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_TDDW_LENGTH,
- 1000,1000,30,100,50);
- AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_TDDW_LENGTH,
- 1000,1000,30,100,50);
- }
- else
- {
- AxisMovePosAccDec(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH,
- 1000,1000,30,100,50);
- AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH,
- 1000,1000,30,100,50);
- }
- CTHL_TD_MotorStep = 18;
- CTHL_TD_MotorDelay = dwTickCount+ 0;
- bFZ_LengthOK = 1;
- }
- break;
- case 18:
- if(!X_DRV&&!Y_DRV)
- {
- CTHL_TD_MotorStep = 0;
- }
- break;
- //由公分节调转
- case 50:
- if(CTHL_FK_Check_DW)
- {
- CTHL_TD_MotorStep = 51;
- CTHL_TD_MotorDelay = dwTickCount + 100;
- bFZ_LengthOK = 0;
- CTHL_GF_Change = 1;
- SetPos(X_AXIS, 0);
- CTHL_CTXM_VAVLE=0;
- CTHL_XSavePosBuff3=0;
- }
- break;
- case 51:
- if(dwTickCount >= CTHL_TD_MotorDelay&&!CTHL_FK_Check)
- {
- CTHL_TD_MotorStep = 52;
- }
- break;
- case 52:
- if(dwXRealPos>=CTHL_PARAM_GFJ_LENGTH)
- {
- CTHL_TryCnt = 0;
- CTHL_LianLengthCheck = 0;
- CTHL_LianLengthCheckOld = 0;
- CTHL_TD_MotorStep = 15;
- }
- break;
- }
- #endif
- }
- //穿入电机动作,Y轴
- void CTHL_FZ_Motor(void) //
- {
- #if 1
- //1步开始, 穿链反转长度开始
- //20步开始 插销定位开始
-
- switch(CTHL_FZ_MotorStep)
- {
- //穿入长度位置
- case 1:
- if(dwTickCount >= CTHL_FZ_MotorDelay)
- {
- SetPos(X_AXIS, 0); //启动位置设为0点
- SetPos(Y_AXIS, 0); //启动位置设为0点
- FZ_ALARM_FLG=0;
- //Set_Ctrlmode_trans(Y_AXIS,TARR_MODE);
- if(CTHL_cZipCnt<1)//前两次走
- {
- AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_JXM_SPEED,DIR_N,1000,1000,30,50);
- AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_JXM_SPEED+500,DIR_N,1500,1000,30,50);
- }
- else
- {
- AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_JXM_SPEED,-CTHL_PARAM_XMCR_SLOW_LENGTH,
- 1200,CTHL_PARAM_JXM_SPEED,50,100,0);
- AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_JXM_SPEED+500,-CTHL_PARAM_XMCR_SLOW_LENGTH,
- 1500,CTHL_PARAM_JXM_SPEED,50,100,0);
- }
- CTHL_FZ_MotorStep = 2;
- }
- break;
- case 2:
- if(abs(dwXRealPos) >=CTHL_PARAM_XMCR_SLOW_LENGTH-100){//提前转换模式
- Set_Ctrlmode_trans(Y_AXIS,TARR_MODE);
- }
- if(abs(dwXRealPos) >= CTHL_PARAM_XMCR_SLOW_LENGTH)//穿链结束,开始走链加速
- {
- CTHL_FZ_MotorDelay = dwTickCount+0;
- if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN)
- {
- AxisContinueMoveChangeSpeed(X_AXIS,CTHL_PARAM_FZFirst_SPEED,1000,20,100);
- AxisContinueMoveChangeSpeed(Y_AXIS,CTHL_PARAM_FZFirst_SPEED,1000,20,100);
- }
- else
- {
- //长度=反转长度-慢速长度-减速长度-修正长度;
- CTHL_LENTH = CTHL_LianLengthAutoFZCRcheck-abs(dwXRealPos)-CTHL_PARAM_FZJS_LENGTH;
- AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FZ_Speed,-CTHL_LENTH,
- 1500,CTHL_PARAM_FZDS_Speed,120,100,0);
- AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZ_Speed+120,-CTHL_LENTH,
- 1500,CTHL_PARAM_FZDS_Speed,120,100,0);
- }
- CTHL_FZ_MotorStep++;
- }
- //电机速度差太大
- if(abs(dwXRealPos)>dwYRealPos_com+4500)
- {
- //CTHL_FZ_MotorStep=100;
- //bCxcAuto=0;//停轴
- //SetEn(Y_AXIS, MOTOR_DISEN);
- SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM);
- //CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机卡滞
- }
- break;
- case 3:
- if(dwTickCount> CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH*10)CTHL_TXM_VAVLE=0; //到达可推出侧推布带时
- //if((dwTickCount>CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH*10 ) && !CTHL_TXM_VAVLE && !CTHL_TXM_Limit_IN)CTHL_JXM_VAVLE=0;
- if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN)
- {
- CTHL_FZ_MotorStep++;
- }
- else
- {
- if(abs(dwXRealPos)>=CTHL_LENTH+CTHL_PARAM_XMCR_SLOW_LENGTH)
- {
- AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FZDS_Speed,-CTHL_PARAM_FZJS_LENGTH,
- 1500,1000,120,80,CTHL_PARAM_FZJSDS_LENGTH);
- AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZDS_Speed,-CTHL_PARAM_FZJS_LENGTH,
- 1500,1000,120,80,CTHL_PARAM_FZJSDS_LENGTH);
- CTHL_FZ_MotorStep++;
- }
- }
- //长度超出报警
- if(CTHL_cZipCnt>=1)
- {
- if(abs(dwXRealPos)>= (CTHL_PARAM_WLCDSHEZHI_LENTH+ CTHL_LianLengthAutoFZCRcheck)) //拉带报警长度
- {
- bCxcAuto=1;//停轴
- SetEn(Y_AXIS, MOTOR_DISEN);
- CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning); //穿入方块超出预设长度
- }
- //方块检测异常(光纤)
- if(abs(dwXRealPos) >= (CTHL_LianLengthAutoFZCRcheck + 300))
- {
- bCxcAuto=1;//停轴
- SetEn(Y_AXIS, MOTOR_DISEN);
- CTHL_SetAlarmCode(CTHL_FK_Check_ALARM,bRunning);
- }
- }
- else
- {
- if(abs(dwXRealPos) >= CTHL_PARAM_first_CL_MAX_Length){ //位置大于第一条拉链长度
- bCxcAuto=1;//停轴
- SetEn(Y_AXIS, MOTOR_DISEN);
- CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning); //穿入方块超出预设长度
- }
- }
- //电机速度差太大
- if(abs(dwXRealPos)>dwYRealPos_com+4500)
- {
- //CTHL_FZ_MotorStep=100;
- //bCxcAuto=0;//停轴
- //SetEn(Y_AXIS, MOTOR_DISEN);
- SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM);
- //CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机
- }
- break;
- case 4:
- if(dwTickCount> CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH*10)CTHL_TXM_VAVLE=0; //到达可推出侧推布带时
- //if((dwTickCount>CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH*10) && !CTHL_TXM_VAVLE && !CTHL_TXM_Limit_IN)CTHL_JXM_VAVLE=0;
- if((abs(dwXRealPos) > 500) && CTHL_FKGY_IN_UP)//位置大于100,且方块感应上升沿
- {
- CTHL_XSavePosBuff1 = abs(dwXRealPos);
- if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN){//记录前两条的方块感应长度;
- //停机距离调整
- CTHL_LENTH=CTHL_PARAM_FZSTOP_LENGTH;
- AxisMovePosAccDec(X_AXIS,800,-CTHL_LENTH,
- 800,800,15,25,0);
- AxisMovePosAccDec(Y_AXIS,600,-CTHL_LENTH,
- 800,800,15,25,0);
- }
- else {
- //停机距离调整
- CTHL_LENTH=CTHL_PARAM_FZSTOP_LENGTH;
- AxisMovePosAccDec(X_AXIS,1000,-CTHL_LENTH,
- 800,800,15,25,0);
- AxisMovePosAccDec(Y_AXIS,600,-CTHL_LENTH,
- 800,800,15,25,0);
- }
- CTHL_FZ_MotorStep++;
- }
- //长度超出报警
- if(CTHL_cZipCnt>=1)
- {
- if(abs(dwXRealPos)>= (CTHL_PARAM_WLCDSHEZHI_LENTH+ CTHL_LianLengthAutoFZCRcheck)) //拉带报警长度
- {
- bCxcAuto=1;//停轴
- SetEn(Y_AXIS, MOTOR_DISEN);
- CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning); //穿入方块超出预设长度
- }
- //方块检测异常(光纤)
- if(abs(dwXRealPos) >= (CTHL_LianLengthAutoFZCRcheck + 300))
- {
- bCxcAuto=1;//停轴
- SetEn(Y_AXIS, MOTOR_DISEN);
- CTHL_SetAlarmCode(CTHL_FK_Check_ALARM,bRunning);
- }
- }
- else
- {
- if(abs(dwXRealPos) >= CTHL_PARAM_first_CL_MAX_Length){ //位置大于第一条拉链长度
- bCxcAuto=1;//停轴
- CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning); //穿入方块超出预设长度
- }
- }
- //电机速度差太大
- if((abs(dwXRealPos)>dwYRealPos_com+4500)&&!FZ_ALARM_FLG)
- {
- //CTHL_FZ_MotorStep=100;
- //bCxcAuto=0;//停轴
- //SetEn(Y_AXIS, MOTOR_DISEN);
- SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM);
- //CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机
- }
- break;
- case 5:
- if(!Y_DRV && !X_DRV)
- {
- if((CTHL_cZipCnt <= 1))
- {
- CTHL_LianLengthCheck = abs(dwXRealPos); //为第一条长度给值
- CTHL_LianLengthAutoFZCRcheck = abs(dwXRealPos);
- CTHL_LianLengthAutoFZcheck=abs(dwXRealPos);
- }
- Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
- CTHL_FZ_MotorStep = 0;
- }
- break;
- //20步开始小插定位
- case 20: //此处最好算好定位长度相减
- if(dwTickCount >= CTHL_FZ_MotorDelay)
- {
- CTHL_FZ_MotorStep = 21;
- SetPos(Y_AXIS, 0); //启动位置设为0点
- SetPos(X_AXIS, 0); //启动位置设为0点
- }
- break;
- case 21:
- if(CTHL_PARAM_CXDW_Length)
- {
- CTHL_FZ_MotorStep = 22;
- AxisMovePosAccDec(Y_AXIS,2*CTHL_PARAM_XCCR_SPEED,-CTHL_PARAM_CXDW_Length,1000,1000,20,20,0);
- AxisMovePosAccDec(X_AXIS,2*CTHL_PARAM_XCCR_SPEED,-CTHL_PARAM_CXDW_Length,1000,1000,20,20,0);
-
- }
- break;
- case 22:
- if(!Y_DRV && !X_AXIS )
- {
- CTHL_FZ_MotorStep = 0;
- Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
- }
- break;
- case 100:
- if(dwYRealPos_com>=CTHL_PARAM_XMCR_SLOW_LENGTH+100&&dwYRealPos_com<=CTHL_LianLengthAutoFZCRcheck-500)//穿过慢速长度
- {
- SetPos(Y_AXIS, 0);
- CTHL_FZ_MotorStep = 101;
- CTHL_XSavePosBuff1=dwYRealPos_com;
- }
- else
- {
- bCxcAuto=0;//停机
- CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机卡滞
- }
- break;
- case 101:
- CTHL_FZ_MotorStep = 102;
- Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
- CTHL_FZ_MotorDelay=dwTickCount+50;
- break;
- case 102:
- CTHL_FZ_MotorStep = 103;
- AxisMovePosAccDec(Y_AXIS,1500,-500,1500,1500,20,20,0);
- break;
- case 103:
- if(!Y_DRV)
- {
- if(dwYRealPos_com>=CTHL_XSavePosBuff1+500)
- {
- Set_Ctrlmode_trans(Y_AXIS,TARR_MODE);
- SetAlarmCode(CTHL_ALARM_ADDR,0);
- CTHL_FZ_MotorStep = 4;
- FZ_ALARM_FLG=1;
- }
- else
- {
- bCxcAuto=0;//停机
- CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机卡滞
- }
- }
- break;
- }
-
-
- #endif
- }
- //装拉头动作
- void CTHL_ZLT_Step(void)
- {
-
- switch(CTHL_ZhuangLiaoStep)
- {
- case 1:
-
- if(bZhuangLiaoOkFlg)
- {
- CTHL_ZhuangLiaoStep = 0;
- }
- else
- {
- if(((CTHL_MGuo_VAVLE && CTHL_LTou_Check) || (!CTHL_MGuo_VAVLE))&&CTHL_GDLT_VAVLE )//说明没有拉头
- {
- /*if(bRunning&&CTXM_DOWN_TIME&&CTHL_cZipCnt>1)
- {
- if(CTHL_TLiaoStep == 0)
- {
- CTHL_TLiaoStep = 1;
- CTHL_ZhuangLiaoStep = 40;
- }
- }
- else*/
- {
- //穿头模和一次穿入必须在原位,接料不能有输出
- if(CTHL_CTM_Origin_IN && !CTHL_CTXM_VAVLE)
- {
- CTHL_JLTou_VAVLE = 0;
- CTHL_GDLT_VAVLE=1;
- CTHL_MGuo_VAVLE = 0;
- if(CTHL_TLiaoStep == 0)
- {
- CTHL_TLiaoStep = 1;
- CTHL_ZhuangLiaoStep = 2;
- }
- }
- else if(dwTickCount >= CTHL_ZhuangLiaoDelay + 1000) //警告横送拉必须保证穿头模和一次穿入在原位,接料不能有输出
- {
- CTHL_wltzi=1;
- CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,bRunning);
-
- }
- }
- }
- else if(dwTickCount >= CTHL_ZhuangLiaoDelay)//已经有拉头直接跳到结束
- {
- CTHL_ZhuangLiaoDelay = dwTickCount + 40;
- CTHL_ZhuangLiaoStep = 9;
- }
- }
- break;
- /* case 40:
- if(CTHL_TLiaoStep == 0) //推料已经完成
- {
- CTHL_HSLiao_VAVLE = 1;
- CTHL_ZhuangLiaoStep = 41;
- CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
- }
- break;
- case 41:
- if(!CTHL_HSLiao_Origin_IN&&!CTHL_CTM_Minid_IN)//离开原点
- {
- CTHL_ZhuangLiaoStep = 42;
- CTHL_ZhuangLiaoDelay=dwTickCount+HSLIAO_CHECK_TIME+1;//横送过程检测
-
- }
- break;
- case 42:
- if(dwTickCount>=CTHL_ZhuangLiaoDelay)//兼容没有做缓冲的情况
- {
- if(CTHL_CTM_Origin_IN )//穿头模和第一次合链在原位
- {
- CTHL_ZhuangLiaoStep = 3;
-
- }
- else
- {
- CTHL_ZhuangLiaoStep = 43;
- CTHL_HSLiao_VAVLE = 0;
- CTHL_ZhuangLiaoDelay = dwTickCount + 20;
- }
- }
- break;
- case 43:
- if(dwTickCount >= CTHL_ZhuangLiaoDelay)
- {
- if(CTHL_CTM_Origin_IN )//穿头模和第一次合链在原位
- {
- CTHL_ZhuangLiaoStep = 3;
- CTHL_HSLiao_VAVLE = 1;
- CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
- }
- }
- break;*/
- case 2:
- if(CTHL_TLiaoStep == 0) //推料已经完成
- {
- CTHL_ZhuangLiaoDelay = dwTickCount+ 500;
- CTHL_GDLT_VAVLE = 1;
- CTHL_MGuo_VAVLE = 0;
- if(!CTHL_JLTou_VAVLE)
- {
- CTHL_ZhuangLiaoStep = 3;
- CTHL_HSLiao_VAVLE = 1;
- PRV_HSLIAO_OUT_DELAY=dwTickCount;
- }
- }
- break;
- case 3:
- //横送到位
- if(CTHL_HSLiao_Limit_IN && !CTHL_HSLiao_Origin_IN)
- {
- HSLIAO_OUT_DELAY=dwTickCount-PRV_HSLIAO_OUT_DELAY;
- CTHL_ZhuangLiaoStep = 4;
- CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_H_JLiao_DELAY;
- }
- else if(dwTickCount >= CTHL_ZhuangLiaoDelay)
- {
- CTHL_ZhuangLiaoStep = 30;
-
- }
- break;
- case 4:
- if(dwTickCount >= CTHL_ZhuangLiaoDelay)
- {
- CTHL_ZhuangLiaoStep = 5;
- CTHL_JLTou_VAVLE = 1; //接拉头输出,不需要时间
- CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_JLTOU_DELAY;
- }
- break;
- case 5:
- if(dwTickCount >= CTHL_ZhuangLiaoDelay)
- {
- CTHL_ZhuangLiaoStep = 6;
- CTHL_GDLT_VAVLE = 0; //接拉头输出,不需要时间
- CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_GDLT_DELAY;
- }
- break;
- case 6:
- if(dwTickCount >= CTHL_ZhuangLiaoDelay) //延时码勾
- {
- CTHL_MGuo_VAVLE = 1; //码勾输出
- CTHL_ZhuangLiaoStep = 7;
- CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_HSLiaoOff_DELAY;
- bTuiLaTouOkFlg = 0; //认为拉头没法带出
- }
- break;
- case 7:
- if(dwTickCount >= CTHL_ZhuangLiaoDelay)
- {
- CTHL_HSLiao_VAVLE = 0;
- CTHL_ZhuangLiaoStep = 8;
- CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
- }
- break;
- case 8:
- if(!CTHL_HSLiao_Limit_IN) //
- {
- CTHL_ZhuangLiaoStep = 9;
- //CTHL_TLiaoStep = 1; //推拉头
- CTHL_ZhuangLiaoDelay = dwTickCount +CTHL_PARAM_CheckLT_DELAY; //也是给下模能上升的时间
- }
- else if (CTHL_LTou_Check) //时间内信号这说明没有拉头
- {
- //重送3次
- CTHL_TryCnt++;
- CTHL_JLTou_VAVLE = 0; //接拉头关
- CTHL_MGuo_VAVLE = 0; //码勾打开
- CTHL_GDLT_VAVLE = 1;
- if(CTHL_TryCnt == 1)//重新横送一次
- {
- CTHL_ZhuangLiaoStep = 21; //重新送3次
- CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
- }
- else if(CTHL_TryCnt == 2||CTHL_TryCnt == 3)//重新装料2次
- {
- CTHL_ZhuangLiaoStep = 20; //重新送3次
- CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
- }
- else if(CTHL_TryCnt == 4)
- {
- CTHL_TryCnt=0;
- bZhuangLiaoOkFlg = 0;
- CTHL_ZhuangLiaoStep = 0;
- CTHL_wltzi=1;
- user_datas[127]=5;
- CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning);
-
- }
- }
- else if(dwTickCount >= CTHL_ZhuangLiaoDelay)
- {
- CTHL_wltzi=1;
- CTHL_SetAlarmCode(CTHL_HSLiao_Limit_ALARM,bRunning);
- if(bRunning)
- {
- CTHL_ZhuangLiaoStep = 0 ;
- }
- }
- break;
- case 9:
- if(CTHL_LTou_Check) //时间内信号这说明没有拉头
- {
- CTHL_TryCnt++;
- CTHL_JLTou_VAVLE = 0; //接拉头关
- CTHL_MGuo_VAVLE = 0; //码勾打开
- CTHL_GDLT_VAVLE = 1;
- if(CTHL_TryCnt == 1)//重新横送一次
- {
- CTHL_ZhuangLiaoStep = 21; //重新送3次
- CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
- }
- else if(CTHL_TryCnt == 2||CTHL_TryCnt == 3)//重新装料2次
- {
- CTHL_ZhuangLiaoStep = 20; //重新送3次
- CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
- }
- else if(CTHL_TryCnt == 4)
- {
- CTHL_TryCnt=0;
- bZhuangLiaoOkFlg = 0;
- CTHL_ZhuangLiaoStep = 0;
- CTHL_wltzi=1;
- user_datas[127]=5;
- CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning);
-
- }
- }
- else if(dwTickCount >= CTHL_ZhuangLiaoDelay)
- {
- bZhuangLiaoOkFlg = 1;
- CTHL_TLiaoStep = 1; //推拉头
- CTHL_ZhuangLiaoStep = 0;
- }
- break;
- //装拉头2次
- case 20:
- if(CTHL_HSLiao_Origin_IN)
- {
- CTHL_JLTou_VAVLE = 0; //接拉头关
- CTHL_MGuo_VAVLE = 0; //码勾打开
- CTHL_GDLT_VAVLE = 1;
- CTHL_ZhuangLiaoStep = 1;
- CTHL_ZhuangLiaoDelay = dwTickCount + 10;
- }
- else if(dwTickCount >=CTHL_ZhuangLiaoDelay)
- {
- CTHL_wltzi=1;
- CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning);
- }
- break;
- case 21: //重新横送一次
- if(CTHL_HSLiao_Origin_IN)
- {
- CTHL_JLTou_VAVLE = 0; //接拉头关
- CTHL_MGuo_VAVLE = 0; //码勾打开
- CTHL_GDLT_VAVLE = 1;
- CTHL_ZhuangLiaoStep = 2;
- CTHL_ZhuangLiaoDelay = dwTickCount + 10;
- }
- else if(dwTickCount >=CTHL_ZhuangLiaoDelay)
- {
- CTHL_wltzi=1;
- CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning);
- }
- break;
- case 30:
- CTHL_HSLiao_VAVLE=0;
- CTHL_ZhuangLiaoStep = 31;
- CTHL_ZhuangLiaoDelay = dwTickCount + 200;
- break;
- case 31:
- if(dwTickCount >=CTHL_ZhuangLiaoDelay)
- {
- CTHL_HSLiao_VAVLE=1;
- CTHL_ZhuangLiaoStep = 32;
- CTHL_ZhuangLiaoDelay = dwTickCount + 3000;
-
- }
- break;
- case 32:
- if(CTHL_HSLiao_Limit_IN && !CTHL_HSLiao_Origin_IN)
- {
- CTHL_ZhuangLiaoStep = 4;
- CTHL_ZhuangLiaoDelay = dwTickCount + 10;
- }
-
- else if(dwTickCount >= CTHL_ZhuangLiaoDelay)
- {
-
- CTHL_wltzi=1;
- CTHL_SetAlarmCode(CTHL_HSLiao_Limit_ALARM,bRunning);
- CTHL_ZhuangLiaoStep = 0;
- CTHL_HSLiao_VAVLE = 0;
- }
- }
- }
- void CTHL_TLiao_Step(void)
- {
- switch(CTHL_TLiaoStep)
- {
- case 1:
- if(bTuiLaTouOkFlg)
- {
- CTHL_TLiaoStep = 0;
- }
- else
- {
- CTHL_TLiao_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
- CTHL_TLiaoStep = 2;
- }
- break;
- case 2: //横送料在原位
- if(CTHL_HSLiao_Origin_IN && !CTHL_HSLiao_Limit_IN && !CTHL_HSLiao_VAVLE)
- {
- CTHL_TLiao_VAVLE = 1; //推料(推拉头)输出
- CTHL_TLiao_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
- CTHL_TLiaoStep = 3;
- }
- else if(dwTickCount >= CTHL_TLiao_Delay)
- {
- CTHL_wltzi=1;
- CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,1);
-
- }
- break;
- case 3: //推拉头到位
- if(CTHL_TLiao_Limit_IN)
- {
- CTHL_TLiao_Delay = dwTickCount + CTHL_PARAM_TLiao_BACKDELAY;
- CTHL_TLiaoStep = 4;
- }
- else if(dwTickCount >= CTHL_TLiao_Delay)
- {
- CTHL_wltzi=1;
- CTHL_SetAlarmCode(CTHL_TLiao_Limit_ALARM,1);
-
- }
- break;
- case 4:
- if(dwTickCount >= CTHL_TLiao_Delay)
- {
- CTHL_TLiao_VAVLE = 0;
- CTHL_TLiao_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
- CTHL_TLiaoStep = 5;
- }
- break;
- case 5:
- if(!CTHL_TLiao_Limit_IN)
- {
- CTHL_TLiaoStep = 0;
- CTHL_TLiao_Delay = dwTickCount;
- bTuiLaTouOkFlg = 1;
- }
- else if(dwTickCount >= CTHL_TLiao_Delay)
- {
- CTHL_wltzi=1;
- CTHL_SetAlarmCode(CTHL_TLiao_Limit_ALARM,0);
-
- }
- break;
- }
-
- }
- //振动盘控制
- void CTHL_ZhenDongAction(void)
- {
-
- #if 0
-
- // if(bRunning)
- // {
- if(!CTHL_PARAM_ZDP_AUTO)
- {
-
- if(CTHL_ZhenDongPian_OUT == 0)
- {
- if(!CTHL_ZDP_IN)
- {
- if(dwTickCount >= CTHL_ZDP_Delay)
- {
- CTHL_ZhenDongPian_OUT = 1;
- CTHL_ZDP_Delay = dwTickCount + CTHL_PARAM_ZDP_Stop_TIME;//CTHL_PARAM_ZDP_Stop_TIME + 50;
- }
- }
- else
- {
- CTHL_ZDP_Delay = dwTickCount + CTHL_PARAM_ZDP_Start_TIME;//CTHL_PARAM_ZDP_Start_TIME;
- }
- }
- else
- {
- if(CTHL_ZDP_IN)
- {
- if(dwTickCount >= CTHL_ZDP_Delay)
- {
- CTHL_ZhenDongPian_OUT = 0;
- CTHL_ZDP_Delay = dwTickCount + CTHL_PARAM_ZDP_Start_TIME;//CTHL_PARAM_ZDP_Start_TIME
- }
- }
- else
- CTHL_ZDP_Delay = dwTickCount + CTHL_PARAM_ZDP_Stop_TIME;//CTHL_PARAM_ZDP_Stop_TIME+ 50;
- }
-
- }
-
- else if (CTHL_PARAM_ZDP_AUTO ==1 )
- {
- CTHL_ZhenDongPian_OUT=0;
- CTHL_ZDP_Delay = dwTickCount + 0;
- }
- else if(CTHL_PARAM_ZDP_AUTO == 2)
- {
- CTHL_ZhenDongPian_OUT=1;
- CTHL_ZDP_Delay = dwTickCount + 0;
-
- }
- #endif
- }
- unsigned char Manu_HSL_STEP;
- unsigned long Manu_HSL_DELAY;
- void CTHL_Manu_HSongLiao(void)
- {
- switch(Manu_HSL_STEP)
- {
- case 1:
- if(dwTickCount>=Manu_HSL_DELAY)
- {
- CTHL_MGuo_VAVLE=1;
- Manu_HSL_STEP=2;
- Manu_HSL_DELAY=dwTickCount+1000;
- }
- break;
- case 2:
- if(CTHL_MGuo_VAVLE&&CTHL_LTou_Check)//无拉头
- {
- CTHL_GDLT_VAVLE=1;
- CTHL_MGuo_VAVLE=0;
- Manu_HSL_DELAY=dwTickCount+CTHL_VAVLE_ERROR_TIME;
- Manu_HSL_STEP=3;
- }
- else if(dwTickCount>=Manu_HSL_DELAY)
- {
- Manu_HSL_STEP=0;
- CTHL_SetAlarmCode(CTHL_YLT_CHECK_ALARM,0);//夹拉头异常
- }
- break;
- case 3:
- if(CTHL_CTM_Origin_IN &&!CTHL_TLiao_Limit_IN)//下模在原位
- {
- CTHL_HSLiao_VAVLE = 1;
- Manu_HSL_STEP=0;
- }
- else if(dwTickCount>=Manu_HSL_DELAY)
- {
- Manu_HSL_STEP=0;
- if(!CTHL_CTM_Origin_IN)CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,0);
- if(CTHL_TLiao_Limit_IN)CTHL_SetAlarmCode(CTHL_TLiao_Limit_ALARM,0);
-
- }
- break;
- }
- }
- //手动动作
- void CTHL_ManualAction(void)
- {
-
- if(CTHL_bBaoJin) //报警清除加松轴
- {
- CTHL_bBaoJin=0;
- SetEn(Y_AXIS, MOTOR_EN);
- SetEn(X_AXIS, MOTOR_EN);
- SetAlarmCode(CTHL_ALARM_ADDR,0);
- if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE)
- {
- SetClr(X_AXIS, MOTOR_NOALARM);
- SetClr(Y_AXIS, MOTOR_NOALARM);
- CTHL_bSFBBAOJIN_Delay= dwTickCount + 500;
- }
- if(CTHL_PARAM_DBSF_MODE)
- {
- if(!CTHL_SF_ALR_IN)
- {
- CTHL_SF_ALR_CLR=1;
- CTHL_bSFBBAOJIN_Delay= dwTickCount + 500;
-
- }
- }
- }
-
- if(bRunning == 0)
- {
- CTHL_Manu_HSongLiao();
- if(bClearTotal) //切断计数清零
- {
- bClearTotal = 0;
- ClrcToTal(QDCT_TOTAL_ADDR);
- }
- if(CTHL_bHSL) //手动横送料
- {
- CTHL_bHSL= 0;
- if(CTHL_HSLiao_VAVLE)CTHL_HSLiao_VAVLE = 0;//~XY_CTHL_HSLiao_VAVLE;
- else
- {
- CTHL_CTXM_VAVLE=0;
- CTHL_TLiao_VAVLE=0;
- CTHL_JLTou_VAVLE=0;
- CTHL_GDLT_VAVLE=0;
- if(Manu_HSL_STEP==0)Manu_HSL_STEP=1;
- Manu_HSL_DELAY=dwTickCount+20;
- }
- }
-
- if(CTHL_bTLiao ) //手动推料(推拉头)
- {
- CTHL_bTLiao = 0;
- if(CTHL_TLiao_VAVLE)CTHL_TLiao_VAVLE = 0;
- else if(!CTHL_HSLiao_VAVLE && CTHL_HSLiao_Origin_IN)CTHL_TLiao_VAVLE = 1;
- else CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,0); // 横送料不能输出//条件警告
-
- }
- if(CTHL_bJLT)//接拉头
- {
- CTHL_bJLT = 0;
- if(CTHL_JLTou_VAVLE)CTHL_JLTou_VAVLE = 0;
- else if( !CTHL_CTXM_VAVLE && CTHL_CTM_Origin_IN)
- {
- CTHL_MGuo_VAVLE = 0;
- CTHL_JLTou_VAVLE = 1;
- }
- else//条件警告
- {
- CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,0);
- }
- }
- if(CTHL_bMG) //码勾
- {
- CTHL_bMG = 0;
- CTHL_MGuo_VAVLE = !CTHL_MGuo_VAVLE;
- }
- if(CTHL_bGDLT)//夹拉头
- {
- CTHL_bGDLT = 0;
- if(!CTHL_GDLT_VAVLE)CTHL_MGuo_VAVLE = 0;
- CTHL_GDLT_VAVLE = !CTHL_GDLT_VAVLE;
- }
-
- if(CTHL_bJXM) //夹前码
- {
- CTHL_bJXM = 0;
- CTHL_JXM_VAVLE = !CTHL_JXM_VAVLE;
- }
-
- if(CTHL_bTXM) //推斜码
- {
- CTHL_bTXM = 0;
- if((CTHL_JXM_VAVLE || !CTHL_CTXM_VAVLE) && !CTHL_TXM_VAVLE && !CTHL_TFK_VAVLE)
- {
- CTHL_TXM_VAVLE=1;
- }
- else if(CTHL_TXM_VAVLE)
- {
- CTHL_TXM_VAVLE=0;
- }
- else
- CTHL_SetAlarmCode(CTHL_JXM_Limit_ALARM,0); //夹前码要输出
-
- }
- if(CTHL_bQMDW) //前码定位
- {
- CTHL_bQMDW = 0;
- CTHL_QMDW_VAVLE = !CTHL_QMDW_VAVLE;
- }
-
-
- if(CTHL_bTFK) //推方块
- {
- CTHL_bTFK = 0;
- if(!CTHL_TFK_VAVLE && !CTHL_TXM_VAVLE)
- CTHL_TFK_VAVLE = 1;
- else if(CTHL_TFK_VAVLE)
- CTHL_TFK_VAVLE = 0;
-
- }
- if(CTHL_bFKDW) //方块检测
- {
- CTHL_bFKDW = 0;
- //CTHL_FKDW_VAVLE = !CTHL_FKDW_VAVLE;
- }
- if(CTHL_bHL) //合链
- {
- CTHL_bHL = 0;
- CTHL_HL_VAVLE = !CTHL_HL_VAVLE;
- }
- if(CTHL_bYFK) //压方块
- {
- CTHL_bYFK = 0;
- CTHL_YFK_VAVLE = !CTHL_YFK_VAVLE;
- }
- if(CTHL_bYCX) //压小插
- {
- CTHL_bYCX = 0;
- CTHL_YXC_VAVLE = !CTHL_YXC_VAVLE;
- }
- if(CTHL_bXCDW) //插销定位
- {
- CTHL_bXCDW = 0;
- CTHL_XCDW_VAVLE = !CTHL_XCDW_VAVLE;
- }
- if(CTHL_bXCBI) //小插摆臂
- {
- SetEn(X_AXIS, MOTOR_EN);
- SetEn(Y_AXIS, MOTOR_EN);
- CTHL_bXCBI = 0;
- CTHL_JCBI_VAVLE = !CTHL_JCBI_VAVLE;
- }
-
- if(CTHL_bCXDD) //小插摆臂
- {
- CTHL_bCXDD = 0;
- CTHL_DingDai_VAVLE = !CTHL_DingDai_VAVLE;
- }
-
- if(CTHL_bCTXM) //下模
- {
- CTHL_bCTXM = 0;
- CTHL_TXM_VAVLE=0;
- if(CTHL_CTXM_VAVLE)
- {
- CTHL_CTXM_VAVLE = 0;
- CTHL_JLTou_VAVLE = 0;
- CTHL_JXM_VAVLE=0;
- }
- else if(!CTHL_HSLiao_VAVLE && CTHL_HSLiao_Origin_IN && (CTHL_JXM_VAVLE || !CTHL_TXM_VAVLE))
- {
-
- CTHL_CTXM_VAVLE = 1;
- CTHL_JLTou_VAVLE=0;
- CTHL_JXM_VAVLE=1;
- }
- }
- //自动装料
- if(CTHL_bAutoZhuangLiao)
- {
- CTHL_bAutoZhuangLiao = 0;
- if(CTHL_ZhuangLiaoStep == 0)
- {
- bTuiLaTouOkFlg = 1;
- bZhuangLiaoOkFlg = 0;
- CTHL_ZhuangLiaoStep = 1;
- CTHL_MGuo_VAVLE = 1; //判断是否已经有料
- CTHL_GDLT_VAVLE=0;
- CTHL_TryCnt = 0;
- CTHL_ZhuangLiaoDelay = dwTickCount + 1000;
- }
- }
- if(CTHL_bYJDW) //一键定位
- {
- CTHL_bYJDW=0;
- //CTHL_FKDW_VAVLE = 0;
- CTHL_YFK_VAVLE=0;
- CTHL_YXC_VAVLE=0;
- CTHL_HL_VAVLE=0;
- CTHL_CTXM_VAVLE=0;
- CTHL_TXM_VAVLE=0;
- CTHL_JCBI_VAVLE=0;
- CTHL_QMDW_VAVLE=0;
- CTHL_TD_MotorDelay = dwTickCount + 100;
- if(CTHL_TD_MotorStep==0)
- {
- CTHL_CTXM_VAVLE =0;
- if(GetEn(X_AXIS) == MOTOR_EN)
- {
- SetEn(X_AXIS, MOTOR_DISEN);
- SetEn(Y_AXIS, MOTOR_DISEN);
- CTHL_TD_MotorDelay = dwTickCount + 200;
- }
- else
- CTHL_TD_MotorDelay = dwTickCount + 100;
- CTHL_TD_MotorStep=1;
- }
- }
-
- //X轴Y轴电机测试
- if(CTHL_bCLMotor_P)
- {
-
- SetEn(Y_AXIS, MOTOR_DISEN);
- SetEn(X_AXIS, MOTOR_DISEN);
- //CTHL_FKDW_VAVLE=0;
- if(CTHL_FK_Check_UP)
- {
- SetPos(X_AXIS, 0);
- SetPos(Y_AXIS, 0);
- }
-
- if(!X_DRV)
- {
- // Y轴 运行速度 启动速度 加速度 减速度
- AxisContinueMoveAcc(X_AXIS,600 ,DIR_P,600,600,15,15);
- AxisContinueMoveAcc(Y_AXIS,580,DIR_P,580,580,15,15);
- }
- }
-
- //在压方块电磁阀输出情况下,方块感应后电机要自动停止
- if(CTHL_FKGY_IN && CTHL_YFK_VAVLE && CTHL_bCLMotor_P)
- {
- CTHL_bCLMotor_P=0;
- AxisEgmStop(Y_AXIS);
- AxisEgmStop(X_AXIS);
- }
-
- if(CTHL_bCLMotor_N) //后退限位已经取消
- {
- SetEn(Y_AXIS, MOTOR_DISEN);
- SetEn(X_AXIS, MOTOR_DISEN);
- //CTHL_FKDW_VAVLE=0;
- if(CTHL_FK_Check_UP)
- {
- SetPos(X_AXIS, 0);
- SetPos(Y_AXIS, 0);
- }
- //压方块电磁阀没有输出时,方块感应信号不限制
- if(!CTHL_FKGY_IN || !CTHL_YFK_VAVLE)
- {
- if(!X_DRV && (!CTHL_QMDW_IN ||!CTHL_QMDW_VAVLE))
- {
- // Y轴 运行速度 启动速度 加速度 减速度
- AxisContinueMoveAcc(Y_AXIS,630,DIR_N,630,630,15,15);
- AxisContinueMoveAcc(X_AXIS,600,DIR_N,600,600,15,15);
- }
-
- if(CTHL_FKGY_IN && CTHL_YFK_VAVLE)
- {
- CTHL_bCLMotor_P=0;
- AxisEgmStop(Y_AXIS);
- AxisEgmStop(X_AXIS);
- }
-
- }
- if(CTHL_QMDW_IN && CTHL_QMDW_VAVLE)
- {
- AxisEgmStop(Y_AXIS);
- AxisEgmStop(X_AXIS);
-
- }
- }
- if(!CTHL_bCLMotor_P && !CTHL_bCLMotor_N && (CTHL_TD_MotorStep == 0))
- {
- AxisEgmStop(Y_AXIS);
- AxisEgmStop(X_AXIS);
- }
-
- }
- }
- #endif
|