運動控制卡回原點示意圖及其代碼

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; /*單軸回原點 * 功能:執(zhí)行單軸回原點運動參數(shù)：cardno 卡號axis 軸號 (1-4) backDir 回原點方向0：正向1：負向logical0 回原點stop0設置0:低電平有效1:高電平有效logical1 回原點stop1設

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

5、cal0原點設置 )，找到 stop0; 第二步 :慢速反向離開 stop0，反向移動指定原點范圍脈沖數(shù); 第三步 :再次慢速接近 stop0; 第四步 :慢速接近 stop1(logical1編碼器 Z相). (2) 第四步可以選擇是否執(zhí)行,通過 logical1來選擇 . (3) 若需多軸回原點 ,必須等待上一軸回原點結束后，才能執(zhí)行下一軸的回原點動作. */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; /步驟；驅動狀態(tài)intStopSignal=0; /停止信號long longPulse =2147483647; /長脈沖， 4字節(jié)上限Stop_Flagaxis-1=0; /標記該軸開始運動if (cardno=max_card_number ) return -1;/卡號判斷if(axis4) re

7、turn -1; /軸號判斷if(backDir1) return -1; /方向判斷if(logical0 != 0 & logical0 != 1 ) return -1; /STOP0方式判斷if(logical1 != 0 & logical1 != 1 & logical1 !=- 1) return -1; /STOP1方式判斷/搜索范圍 ,搜索速度 ,每轉脈沖判斷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);/設定stop0有效電平Start_Run (axis); while (TRUE) switch(workstep) case0:/發(fā)送長脈沖/if(Check_OriginSignal(cardno,ax

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

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

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

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

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

14、no,axis,&status); if (status) break;/驅動進行中，繼續(xù)讀信號和狀態(tài)if (StopSignal=0 | StopSignal=2) /未檢測到正常停止信號 set_stop0_mode (cardno,axis,0,0); return -2; if(logical1=-1) /不搜索 Z，回原點完成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相，移動與每轉脈沖數(shù)等長的位移 StopSignal=0; set_startv (cardno,axis,phaseSpeed); /低速接近 Z相set_speed (cardno,axis,phaseSpeed); /低速接近 Z相set_stop1_mode (cardno,axis,1,logical1);/設置Stop1有效電平pmove(cardno,axis,backDir?pulseUnit:

16、-pulseUnit); workstep+; break; case7:/判斷搜索 Z相信號結果 if(Check_OriginSignal(cardno,axis, 1) StopSignal=1;/由STOP1引起get_status (cardno,axis,&status); if (status) break;/驅動進行中，繼續(xù)讀信號和狀態(tài)if (StopSignal=0) /轉了一圈仍然沒有找到 Z信號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)/響應外部的停止信號 ,退出break; /轉讓控制權PeekMessage (&msg,NULL ,0