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);
}
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
}
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);
}
///机器人运行前必须至少进行一次标定操作!
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);
}