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);

}