DMC运动控制器
DMC_21x3控制器，Galil
连接控制器代码
void
CREBot6VDlg::OnButtonConnectController() 
{
///连接运动控制器，并作初始化
///DCM2163 controller object
CDMCWin m_DMCWin; CDMCWinRegistry DMCWinRegistry; GALILREGISTRY galilregistry; ///判断运动控制器是否注册 if (DMCWinRegistry.GetGalilRegistryInfo(m_nController, &galilregistry) != 0) { //SetDlgItemText(IDC_EDIT_SYS_MSG,"请先使用DMC smart Terminal软件正确设置DMC2163运动控制器！"); ShowInfo("请先使用DMC smart Terminal软件正确设置DMC2163运动控制器！",ICON_ALERT); return; } if(galilregistry.fControllerType!=ControllerTypeEthernet||strcmp(galilregistry.szModel,"DMC-21x3/2")!=0) { ShowInfo("请先使用DMC smart Terminal软件正确设置DMC2163运动控制器！并且使用以太网通讯！",ICON_ALERT);// SetDlgItemText(IDC_EDIT_SYS_MSG,"请先使用DMC smart Terminal软件正确设置DMC2163运动控制器！并且使用以太网通讯！"); return; } /// 初始化运动控制器,设置为在注册表中第一个运动控制器 m_DMCWin.SetController(m_nController); m_DMCWin.SethWnd(GetSafeHwnd()); /// 打开和DMC运动控制器间的连接 m_DMCWin.Open(); CString TempString; if(m_DMCWin.IsConnected()) { TempString="已经连接: "; if (m_DMCWin.GetVersion(szResponse, sizeof(szResponse)) == 0) { //SetDlgItemText(IDC_EDIT_SYS_MSG,TempString); TempString+=szResponse; ShowInfo(TempString,ICON_INFO); } ((CStatic *)GetDlgItem(IDC_STATIC_COMU_STATUS))->SetIcon(AfxGetApp()->LoadIcon(IDI_ICON_ROBOT_READY)); } else { /// 返回错误信息 m_DMCWin.GetErrorText(m_DMCWin.GetLastError(),szResponse,sizeof(szResponse)); TempString = "连接运动控制器失败： "; TempString += szResponse; //SetDlgItemText(IDC_EDIT_SYS_MSG,TempString); ShowInfo(TempString,ICON_ERROR); return; } GetDlgItem(IDC_BUTTON_CALIBRATE)->EnableWindow(TRUE); GetDlgItem(IDC_BUTTON_CONNECT_CONTROLLER)->EnableWindow(FALSE);
}
///获取运动控制状态线程
char
*
const
COMMAND_GET_AXIS_POS[
18
]
=
{"MG_TDA\r","MG_LFA\r","MG_LRA\r", "MG_TDB\r","MG_LFB\r","MG_LRB\r", "MG_TDC\r","MG_LFC\r","MG_LRC\r", "MG_TDD\r","MG_LFD\r","MG_LRD\r", "MG_TDE\r","MG_LFE\r","MG_LRE\r", "MG_TDF\r","MG_LFF\r","MG_LRF\r"}
;
bool
gbGetMotionStatusThreadRunning
=
true
;DWORD WINAPI threadProc_GetMotionStatus( LPVOID pParam )
{ //t_Axis *axisArray=(t_Axis *)pParam; long rc; char szRepaly[4096]; int axisIndex=0; int errorOkCount=0; FILE *fdebug=fopen("d.txt","wt"); while(gbGetMotionStatusThreadRunning) { ::WaitForSingleObject(ghMutexVisit,INFINITE);//当ghMutexVisit变成有信号才返回，否则，一直等下去。 for(axisIndex=0;axisIndex<6;axisIndex++) { rc = m_DMCWin.Command(COMMAND_GET_AXIS_POS[3*axisIndex],szRepaly,sizeof(szRepaly)); if (rc == DMCNOERROR) gCurAxisPulse[axisIndex]=atol(szRepaly); else if (rc == DMCERROR_TIMEOUT) gDMCTimeOutCount++; rc = m_DMCWin.Command(COMMAND_GET_AXIS_POS[3*axisIndex+1],szRepaly,sizeof(szRepaly)); if (rc == DMCNOERROR) gCurAxisForwardLimit[axisIndex]=atoi(szRepaly); else if (rc == DMCERROR_TIMEOUT) gDMCTimeOutCount++; rc = m_DMCWin.Command(COMMAND_GET_AXIS_POS[3*axisIndex+2],szRepaly,sizeof(szRepaly)); if (rc == DMCNOERROR) gCurAxisBackwardLimit[axisIndex]=atoi(szRepaly); else if (rc == DMCERROR_TIMEOUT) gDMCTimeOutCount++; } ///rc = m_DMCWin.Command("MG_BGS\r",szRepaly,sizeof(szRepaly)); if(gbMotionFinished==false) { errorOkCount=0; for(axisIndex=0;axisIndex<6;axisIndex++) { fprintf(fdebug,"%d ",(int)(fabs(gCurAxisPulse[axisIndex]-glLIMoveGoalPulse[axisIndex]))); if(fabs(gCurAxisPulse[axisIndex]-glLIMoveGoalPulse[axisIndex])<CONTROL_ERROR_PULSE) errorOkCount++; } if(errorOkCount==6) gbMotionFinished=true; fprintf(fdebug,"\n");// lVectroError+=(gCurAxisPulse[axisIndex]-glLIMoveGoalPulse[axisIndex])*(gCurAxisPulse[axisIndex]-glLIMoveGoalPulse[axisIndex]);// fprintf(fdebug,"%ld %ld %ld %ld %ld %ld %ld %ld %ld %ld %ld %ld %ld\n",gCurAxisPulse[0],glLIMoveGoalPulse[0],gCurAxisPulse[1],glLIMoveGoalPulse[1],gCurAxisPulse[2],glLIMoveGoalPulse[2],gCurAxisPulse[3],glLIMoveGoalPulse[3],gCurAxisPulse[4],glLIMoveGoalPulse[4],gCurAxisPulse[5],glLIMoveGoalPulse[5],lVectroError);// if(lVectroError<CONTROL_ERROR_PULSE)// gbMotionFinished=true; } ::ReleaseMutex(ghMutexVisit); Sleep(20); } fclose(fdebug); return 0; // thread completed successfully}
从绝对编码器读取值。机器人零点位置标定
///机器人运行前必须至少进行一次标定操作!
void
CREBot6VDlg::OnButtonCalibrate()
{ ///DMC运动控制器已经设置好/* m_DMCWin.Command(DMC_INIT_COMMAND_SEQ[0],szResponse,sizeof(szResponse)); m_DMCWin.Command(DMC_INIT_COMMAND_SEQ[1],szResponse,sizeof(szResponse)); m_DMCWin.Command(DMC_INIT_COMMAND_SEQ[2],szResponse,sizeof(szResponse)); m_DMCWin.Command(DMC_INIT_COMMAND_SEQ[3],szResponse,sizeof(szResponse)); m_DMCWin.Command(DMC_INIT_COMMAND_SEQ[4],szResponse,sizeof(szResponse)); m_DMCWin.Command(DMC_INIT_COMMAND_SEQ[5],szResponse,sizeof(szResponse)); m_DMCWin.Command(DMC_INIT_COMMAND_SEQ[6],szResponse,sizeof(szResponse)); m_DMCWin.Command(DMC_INIT_COMMAND_SEQ[7],szResponse,sizeof(szResponse));*/ ///打开RS232C串口读取绝对编码器值 if(OpenCommPort(COM1,BAUDRATE_19200)==FALSE) { AfxMessageBox("打开RS232C通讯端口失败! 标定没有完成!"); return; } int i; long absPulse[6]; for(i=1;i<=6;i++) { if(ReadAbsPositonFromRS422(i,absPulse[i-1])!=RS422_NOERROR) { AfxMessageBox("绝对编码器读取数据失败!"); CloseCommPort(); return; } absPulse[i-1]=-absPulse[i-1]; Sleep(50); } CloseCommPort(); long rc; //DPABS_1,ABS_2,ABS_3,ABS_4,ABS_5,ABS_6 ///将绝对编码器的值设置为当前运动控制器的规划值和目标值 sprintf(szCommandStr,"DP%d,%d,%d,%d,%d,%d\r",absPulse[0],absPulse[1],absPulse[2],absPulse[3],absPulse[4],absPulse[5]); m_DMCWin.Command(szCommandStr,szResponse,sizeof(szResponse));///for stepper mode,set TP pulse sprintf(szCommandStr,"DE%d,%d,%d,%d,%d,%d\r",absPulse[0],absPulse[1],absPulse[2],absPulse[3],absPulse[4],absPulse[5]); m_DMCWin.Command(szCommandStr,szResponse,sizeof(szResponse));/// ///从配置文件读取较准原点坐标数据 FILE *fin=fopen(ORG_SET_FILE_NAME,"rt"); if(fin==NULL) { AfxMessageBox("打开原点配置文件REOrg.ini失败!\n\n请通过如下步骤执行原点校准：\n\n1.点动调整机器人各个轴位置到标定位置;\n2.点选隐含菜单--保存标定位置;\n3.退出控制程序，重新运行控制程序。"); ShowInfo("运行前必须对机器人绝对位置数据作校准！",ICON_ALERT); return; } int readCount=0; readCount=fscanf(fin,"%ld %ld %ld %ld %ld %ld",&absPulse[0],&absPulse[1],&absPulse[2],&absPulse[3],&absPulse[4],&absPulse[5]); if(readCount!=6) { AfxMessageBox("打开原点配置文件REOrg.ini失败!\n\n请通过如下步骤执行原点校准：\n\n1.点动调整机器人各个轴位置到标定位置;\n2.点选隐含菜单--保存标定位置;\n3.退出控制程序，重新运行控制程序。"); ShowInfo("运行前必须对机器人绝对位置数据作校准！",ICON_ALERT); EnableJOGButtons(TRUE); fclose(fin); return; } fclose(fin); ABS_AXIS_HOME_PULSE[0]=absPulse[0];ABS_AXIS_HOME_PULSE[1]=absPulse[1];ABS_AXIS_HOME_PULSE[2]=absPulse[2]; ABS_AXIS_HOME_PULSE[3]=absPulse[3];ABS_AXIS_HOME_PULSE[4]=absPulse[4];ABS_AXIS_HOME_PULSE[5]=absPulse[5]; if(MessageBox("机器人将依次运动R,U,L,S,T,B六个轴到达标定零点位姿。\n\n在运动过程中请确保所有人员远离机器人作业范围!\n\n如果机器人运动过程不正常请立刻按下急停按钮!\n\n\n确认机器人可以安全动作了吗？", "机器人回标定零点",MB_YESNO|MB_ICONQUESTION)==IDYES) { sprintf(szCommandStr,"PA%ld,%ld,%ld,%ld,%ld,%ld\r",ABS_AXIS_HOME_PULSE[0],ABS_AXIS_HOME_PULSE[1],ABS_AXIS_HOME_PULSE[2],ABS_AXIS_HOME_PULSE[3],ABS_AXIS_HOME_PULSE[4],ABS_AXIS_HOME_PULSE[5]); m_DMCWin.Command(szCommandStr,szResponse,sizeof(szResponse)); sprintf(szCommandStr,"SP20000,30000,30000,50000,50000,50000\r"); m_DMCWin.Command(szCommandStr,szResponse,sizeof(szResponse)); sprintf(szCommandStr,"AC100000,100000,100000,100000,100000,100000\r"); m_DMCWin.Command(szCommandStr,szResponse,sizeof(szResponse)); sprintf(szCommandStr,"DC100000,100000,100000,100000,100000,100000\r"); m_DMCWin.Command(szCommandStr,szResponse,sizeof(szResponse)); for(i=3;i>=0;i--) { m_DMCWin.Command(AXIS_BY_AXIS_MOVE_COMMAND[i],szResponse,sizeof(szResponse)); ///wait motion complete while(1) { rc=m_DMCWin.Command(AXIS_BY_AXIS_MOVE_COMMAND[i+6],szResponse,sizeof(szResponse)); //if(atoi(szResponse)==0) // break; //if(szResponse[0]=='0') // break; if (rc == DMCNOERROR) { if(atoi(szResponse)==0) break; } else if (rc == DMCERROR_TIMEOUT) gDMCTimeOutCount++; } } for(i=4;i<=5;i++) { m_DMCWin.Command(AXIS_BY_AXIS_MOVE_COMMAND[i],szResponse,sizeof(szResponse)); ///wait motion complete while(1) { rc=m_DMCWin.Command(AXIS_BY_AXIS_MOVE_COMMAND[i+6],szResponse,sizeof(szResponse)); //if(atoi(szResponse)==0) // break; //if(szResponse[0]=='0') // break; if (rc == DMCNOERROR) { if(atoi(szResponse)==0) break; } else if (rc == DMCERROR_TIMEOUT) gDMCTimeOutCount++; } } if(MessageBox("请确认机器人S,L,U,R,B,T,六个轴准确到达标定零点位姿?","询问",MB_YESNO|MB_ICONQUESTION)==IDYES) { ShowInfo("机器人完成标定检查过程，现在可以正常操作机器人。",ICON_OK); for(i=0;i<6;i++) { sprintf(szCommandStr,"FL%c=%ld;BL%c=%ld;\r", (char)('A'+i),(long)(ABS_AXIS_HOME_PULSE[i]+gSoftLimitDegree[2*i+1]*gPulsePerDegree[i]),(char)('A'+i),(long)(ABS_AXIS_HOME_PULSE[i]+gSoftLimitDegree[2*i]*gPulsePerDegree[i])); m_DMCWin.Command(szCommandStr,szResponse,sizeof(szResponse)); } } else { AfxMessageBox("机器人绝对原点标定数据不正确!\n\n请通过如下步骤执行原点校准：\n\n1.点动调整机器人各个轴位置到标定位置;\n2.点选隐含菜单--保存标定位置;\n3.退出控制程序，重新运行控制程序。"); ShowInfo("运行前必须对机器人绝对位置数据作校准！",ICON_ALERT); m_DMCWin.Clear(); m_DMCWin.Close(); return; } } else { m_DMCWin.Clear(); //m_DMCWin.Close(); return; } GetDlgItem(IDC_BUTTON_CALIBRATE)->EnableWindow(FALSE); GetDlgItem(IDC_BUTTON_GO_STOP_POSITION)->EnableWindow(TRUE); EnableMotionButtons(TRUE); EnableJOGButtons(TRUE); ///Resume the thread if(m_GetStatusThreadHandle!=NULL) ResumeThread(m_GetStatusThreadHandle); SetTimer(1,150,NULL);}