運(yùn)動(dòng)控制卡回原點(diǎn)示意圖及其代碼

1、int Check_OriginSignal(int cardno,int axis,int mode) if (mode=0) if(!STOP0_Highaxis-1) int bit= 2,8,14,20; /stop0return read_bit(cardno,bitaxis-1)=0?1:0; else int bit= 2,8,14,20; /stop0return read_bit(cardno,bitaxis-1)=1?1:0; else if(!STOP1_Highaxis-1) int bit= 3,9,15,21; /stop1return read_bit(cardn

2、o,bitaxis-1)=0?1:0; else int bit= 3,9,15,21; /stop1return read_bit(cardno,bitaxis-1)=1?1:0; int Check_LimitSignal(int cardno,int axis,int mode) if(!LMTLevel_Highaxis- 1) /低電平有效if (mode) int bit= 0,6,12,18; /LMT-return read_bit(cardno,bitaxis-1)=0?1:0; else int bit= 1,7,13,19; /LMT+return read_bit(ca

3、rdno,bitaxis-1)=0?1:0; else/高電平有效if (mode) int bit= 0,6,12,18; /LMT-return read_bit(cardno,bitaxis-1)=1?1:0; else int bit= 1,7,13,19; /LMT+return read_bit(cardno,bitaxis-1)=1?1:0; /*單軸回原點(diǎn) * 功能:執(zhí)行單軸回原點(diǎn)運(yùn)動(dòng)參數(shù)：cardno 卡號(hào)axis 軸號(hào) (1-4) backDir 回原點(diǎn)方向0：正向1：負(fù)向logical0 回原點(diǎn)stop0設(shè)置0:低電平有效1:高電平有效logical1 回原點(diǎn)stop1設(shè)

4、置0:低電平有效1:高電平有效-1：無效（不搜索 Z相）homeStartV 回原點(diǎn)啟始速度，取值范圍：0-2M homeSpeed 回原點(diǎn)驅(qū)動(dòng)速度，取值范圍：0-2M homeAcc 回原點(diǎn)加速度，取值范圍：0-64000 searchRange 原點(diǎn)范圍(不宜過大 ) searchSpeed stop0搜索速度 (不宜過高 ) phaseSpeed Z相搜索速度(不宜過高 ) pulseUnit 每轉(zhuǎn)脈沖返回值0:回原點(diǎn)成功 ; -1:參數(shù)錯(cuò)誤 ; -2：回原點(diǎn)失敗 ,(碰到限位或原點(diǎn)范圍過小);1：回原點(diǎn)被中止說明: (1) 回原點(diǎn)分為四大步 : 第一步 :快速接近 stop0(logi

5、cal0原點(diǎn)設(shè)置 )，找到 stop0; 第二步 :慢速反向離開 stop0，反向移動(dòng)指定原點(diǎn)范圍脈沖數(shù); 第三步 :再次慢速接近 stop0; 第四步 :慢速接近 stop1(logical1編碼器 Z相). (2) 第四步可以選擇是否執(zhí)行,通過 logical1來選擇 . (3) 若需多軸回原點(diǎn) ,必須等待上一軸回原點(diǎn)結(jié)束后，才能執(zhí)行下一軸的回原點(diǎn)動(dòng)作. */int _stdcall home1(int cardno,int axis,int backDir,int logical0 ,int logical1 ,longhomeStartV,long homeSpeed, long ho

6、meAcc,long searchRange, long searchSpeed, longphaseSpeed , long pulseUnit) staticMSG msg; static int nCount =0; int workstep=0,status=-1,value=-1; /步驟；驅(qū)動(dòng)狀態(tài)intStopSignal=0; /停止信號(hào)long longPulse =2147483647; /長脈沖， 4字節(jié)上限Stop_Flagaxis-1=0; /標(biāo)記該軸開始運(yùn)動(dòng)if (cardno=max_card_number ) return -1;/卡號(hào)判斷if(axis4) re

7、turn -1; /軸號(hào)判斷if(backDir1) return -1; /方向判斷if(logical0 != 0 & logical0 != 1 ) return -1; /STOP0方式判斷if(logical1 != 0 & logical1 != 1 & logical1 !=- 1) return -1; /STOP1方式判斷/搜索范圍 ,搜索速度 ,每轉(zhuǎn)脈沖判斷if(searchRange 0|searchSpeed 0|pulseUnit=homeSpeed) homeStartV=homeSpeed; set_startv (cardno,axis,

8、homeStartV); set_speed (cardno,axis,homeSpeed); else set_startv (cardno,axis,homeStartV); set_speed (cardno,axis,homeSpeed); set_acc (cardno,axis,homeAcc); set_stop0_mode (cardno,axis,1,logical0);/設(shè)定stop0有效電平Start_Run (axis); while (TRUE) switch(workstep) case0:/發(fā)送長脈沖/if(Check_OriginSignal(cardno,ax

9、is,0) /已經(jīng)在原點(diǎn) ,不發(fā)送長脈沖直接反向退出StopSignal=1;/由原點(diǎn)引起workstep=2; break; /pmove(cardno,axis,backDir?-longPulse:longPulse); workstep+; break ; case1:/分析是否碰到原點(diǎn)或限位if (backDir=1 & StopSignal= 0)/負(fù)向回原點(diǎn) if(Check_OriginSignal(cardno,axis, 0) StopSignal=1;/由stop0原點(diǎn)引起if (Check_LimitSignal(cardno,axis, 1) StopSign

10、al=2;/由負(fù)限位引起 if ( backDir=0 & StopSignal= 0)/正向回原點(diǎn) if(Check_OriginSignal(cardno,axis, 0) StopSignal=1;/由原點(diǎn)引起if (Check_LimitSignal (cardno,axis, 0) StopSignal=2;/由正限位引起 get_status (cardno,axis,&status);/驅(qū)動(dòng)狀態(tài)檢測(cè)if (status) break;/驅(qū)動(dòng)進(jìn)行中，繼續(xù)檢測(cè)if (StopSignal=0) /未檢測(cè)到正常停止信號(hào) set_stop0_mode (cardno,ax

11、is,0,0); if (StopSignal=2) /原點(diǎn)模式時(shí)，由限位引起的停止 return -2; set_stop0_mode (cardno,axis,0,0); StopSignal=0; workstep+; break ; case2:/低速經(jīng)過原點(diǎn)set_startv (cardno,axis,searchSpeed); set_speed (cardno,axis,searchSpeed); pmove(cardno,axis,backDir?searchRange:-searchRange);/搜索范圍workstep+; break ; case3:/判斷驅(qū)動(dòng)結(jié)束，設(shè)

12、置原點(diǎn)有效get_status (cardno,axis,&status);/判斷運(yùn)動(dòng)是否停止if (status) break; /驅(qū)動(dòng)進(jìn)行中，繼續(xù)讀信號(hào)和狀態(tài)set_stop0_mode (cardno,axis,1,logical0); /設(shè)定stop0有效電平workstep+; break ; case4:/低速接近原點(diǎn)pmove(cardno,axis,backDir?-searchRange:searchRange); workstep+; break ; case5:/讀信號(hào)和狀態(tài)if (backDir=1 & StopSignal= 0)/負(fù)向回原點(diǎn) if(C

13、heck_OriginSignal(cardno,axis, 0) StopSignal=1;/由stop0 原點(diǎn)引起if (Check_LimitSignal (cardno,axis, 1) StopSignal=2;/由負(fù)限位引起 if ( backDir=0 & StopSignal= 0)/正向回原點(diǎn) if(Check_OriginSignal(cardno,axis, 0) StopSignal=1;/由stop0 原點(diǎn)引起if (Check_LimitSignal (cardno,axis, 0) StopSignal=2;/由正限位引起 get_status (card

14、no,axis,&status); if (status) break;/驅(qū)動(dòng)進(jìn)行中，繼續(xù)讀信號(hào)和狀態(tài)if (StopSignal=0 | StopSignal=2) /未檢測(cè)到正常停止信號(hào) set_stop0_mode (cardno,axis,0,0); return -2; if(logical1=-1) /不搜索 Z，回原點(diǎn)完成set_stop0_mode (cardno,axis,0,0); set_command_pos (cardno,axis,0); set_actual_pos (cardno,axis,0); returnSUCCESSFUL; else if (l

15、ogical1=0|logical1=1)/搜索Z相 set_stop0_mode (cardno,axis, 0,0); workstep+; break; case6: /編碼器 Z相，移動(dòng)與每轉(zhuǎn)脈沖數(shù)等長的位移 StopSignal=0; set_startv (cardno,axis,phaseSpeed); /低速接近 Z相set_speed (cardno,axis,phaseSpeed); /低速接近 Z相set_stop1_mode (cardno,axis,1,logical1);/設(shè)置Stop1有效電平pmove(cardno,axis,backDir?pulseUnit:

16、-pulseUnit); workstep+; break; case7:/判斷搜索 Z相信號(hào)結(jié)果 if(Check_OriginSignal(cardno,axis, 1) StopSignal=1;/由STOP1引起get_status (cardno,axis,&status); if (status) break;/驅(qū)動(dòng)進(jìn)行中，繼續(xù)讀信號(hào)和狀態(tài)if (StopSignal=0) /轉(zhuǎn)了一圈仍然沒有找到 Z信號(hào)set_stop1_mode (cardno,axis, 0,0); return -2; set_stop1_mode (cardno,axis,0,0); set_command_pos (cardno,axis,0); set_actual_pos (cardno,axis,0); returnSUCCESSFUL; if (Stop_Flagaxis-1=1)/響應(yīng)外部的停止信號(hào) ,退出break; /轉(zhuǎn)讓控制權(quán)PeekMessage (&msg,NULL ,0