1.Robot状态和位置的监控，其中两个arm状态(是否有料)，也作为调度的依据。

LAPTOP-SNT8I5JK\Boounion

2025-06-16 fd1333fbf52e00ff8f0b0d51ec0d707a66cc9141

 SourceCode/Bond/Servo/CRobotTask.cpp

@@ -1,6 +1,7 @@
#include "stdafx.h"
#include "CRobotTask.h"
#include "ToolUnits.h"
#include "Log.h"


namespace SERVO {
@@ -13,6 +14,7 @@
      m_timeStored = 0;
      m_timeFinish = 0;
      m_pContext = nullptr;
      m_pEFEM = nullptr;
   }

   CRobotTask::~CRobotTask()
@@ -26,17 +28,17 @@
   std::string CRobotTask::getDescription() const
   {
      std::string strOut = "CRobotTask<ID:";
      strOut = strOut + std::to_string(m_robotCmdParam.sequenceNo);
      strOut = strOut + std::to_string(m_robotCmdParam[ACTION_TRANSFER].sequenceNo);
      strOut = strOut + ",Arm:";
      strOut = strOut + std::to_string(m_robotCmdParam.armNo);
      strOut = strOut + std::to_string(m_robotCmdParam[ACTION_TRANSFER].armNo);
      strOut = strOut + ",GetPossion:";
      strOut = strOut + std::to_string(m_robotCmdParam.getPosition);
      strOut = strOut + std::to_string(m_robotCmdParam[ACTION_TRANSFER].getPosition);
      strOut = strOut + ",GetSlot:";
      strOut = strOut + std::to_string(m_robotCmdParam.getSlotNo);
      strOut = strOut + std::to_string(m_robotCmdParam[ACTION_TRANSFER].getSlotNo);
      strOut = strOut + ",PutPossion:";
      strOut = strOut + std::to_string(m_robotCmdParam.putPosition);
      strOut = strOut + std::to_string(m_robotCmdParam[ACTION_TRANSFER].putPosition);
      strOut = strOut + ",PutSlot:";
      strOut = strOut + std::to_string(m_robotCmdParam.putSlotNo);
      strOut = strOut + std::to_string(m_robotCmdParam[ACTION_TRANSFER].putSlotNo);
      strOut = strOut + ">";

      return strOut;
@@ -45,17 +47,17 @@
   std::string CRobotTask::getSimpleDescription() const
   {
      std::string strOut = "CRobotTask<ID:";
      strOut = strOut + std::to_string(m_robotCmdParam.sequenceNo);
      strOut = strOut + std::to_string(m_robotCmdParam[ACTION_TRANSFER].sequenceNo);
      strOut = strOut + ",Arm:";
      strOut = strOut + std::to_string(m_robotCmdParam.armNo);
      strOut = strOut + std::to_string(m_robotCmdParam[ACTION_TRANSFER].armNo);
      strOut = strOut + ",GetPossion:";
      strOut = strOut + std::to_string(m_robotCmdParam.getPosition);
      strOut = strOut + std::to_string(m_robotCmdParam[ACTION_TRANSFER].getPosition);
      strOut = strOut + ",GetSlot:";
      strOut = strOut + std::to_string(m_robotCmdParam.getSlotNo);
      strOut = strOut + std::to_string(m_robotCmdParam[ACTION_TRANSFER].getSlotNo);
      strOut = strOut + ",PutPossion:";
      strOut = strOut + std::to_string(m_robotCmdParam.putPosition);
      strOut = strOut + std::to_string(m_robotCmdParam[ACTION_TRANSFER].putPosition);
      strOut = strOut + ",PutSlot:";
      strOut = strOut + std::to_string(m_robotCmdParam.putSlotNo);
      strOut = strOut + std::to_string(m_robotCmdParam[ACTION_TRANSFER].putSlotNo);
      strOut = strOut + ">";

      return strOut;
@@ -63,8 +65,8 @@

   void CRobotTask::setContext(CContext* pContext)
   {
      if (pContext != nullptr) {
         pContext->release();
      if (m_pContext != nullptr) {
         m_pContext->release();
      }

      m_pContext = pContext;
@@ -76,6 +78,11 @@
   CContext* CRobotTask::getContext()
   {
      return m_pContext;
   }

   void CRobotTask::setEFEM(CEFEM* pEFEM)
   {
      m_pEFEM = pEFEM;
   }

   std::string& CRobotTask::generateId(std::string& out)
@@ -97,21 +104,47 @@
      return m_strId;
   }

   void CRobotTask::setRobotTransferParam(int seq, int armNo, int fromPos, int toPos, int fromSlot, int toSlot)
   int CRobotTask::setRobotTransferParam(int seq, int armNo, int fromPos, int toPos, int fromSlot, int toSlot)
   {
      m_robotCmdParam = {};
      m_robotCmdParam.sequenceNo = static_cast<short>(seq);
      m_robotCmdParam.rcmd = static_cast<short>(SERVO::RCMD::Transfer);
      m_robotCmdParam.armNo = static_cast<short>(armNo);
      m_robotCmdParam.getPosition = static_cast<short>(fromPos);
      m_robotCmdParam.getSlotNo = static_cast<short>(fromSlot);
      m_robotCmdParam.putPosition = static_cast<short>(toPos);
      m_robotCmdParam.putSlotNo = static_cast<short>(toSlot);
      // 这是直接使用搬运模式的参数，
      // 如果EFEM不支持，还需要拆解为取和放的动作及回放原位置的其它3套参数
      m_robotCmdParam[ACTION_TRANSFER] = {};
      m_robotCmdParam[ACTION_TRANSFER].sequenceNo = static_cast<short>(seq+1);
      m_robotCmdParam[ACTION_TRANSFER].rcmd = static_cast<short>(SERVO::RCMD::Transfer);
      m_robotCmdParam[ACTION_TRANSFER].armNo = static_cast<short>(armNo);
      m_robotCmdParam[ACTION_TRANSFER].getPosition = static_cast<short>(fromPos);
      m_robotCmdParam[ACTION_TRANSFER].getSlotNo = static_cast<short>(fromSlot);
      m_robotCmdParam[ACTION_TRANSFER].putPosition = static_cast<short>(toPos);
      m_robotCmdParam[ACTION_TRANSFER].putSlotNo = static_cast<short>(toSlot);

      m_robotCmdParam[ACTION_PICK] = {};
      m_robotCmdParam[ACTION_PICK].sequenceNo = static_cast<short>(seq+2);
      m_robotCmdParam[ACTION_PICK].rcmd = static_cast<short>(SERVO::RCMD::Get);
      m_robotCmdParam[ACTION_PICK].armNo = static_cast<short>(armNo);
      m_robotCmdParam[ACTION_PICK].getPosition = static_cast<short>(fromPos);
      m_robotCmdParam[ACTION_PICK].getSlotNo = static_cast<short>(fromSlot);

      m_robotCmdParam[ACTION_PLACE] = {};
      m_robotCmdParam[ACTION_PLACE].sequenceNo = static_cast<short>(seq + 2);
      m_robotCmdParam[ACTION_PLACE].rcmd = static_cast<short>(SERVO::RCMD::Put);
      m_robotCmdParam[ACTION_PLACE].armNo = static_cast<short>(armNo);
      m_robotCmdParam[ACTION_PLACE].putPosition = static_cast<short>(toPos);
      m_robotCmdParam[ACTION_PLACE].putSlotNo = static_cast<short>(toSlot);

      m_robotCmdParam[ACTION_RESTORE] = {};
      m_robotCmdParam[ACTION_RESTORE].sequenceNo = static_cast<short>(seq + 1);
      m_robotCmdParam[ACTION_RESTORE].rcmd = static_cast<short>(SERVO::RCMD::Put);
      m_robotCmdParam[ACTION_RESTORE].armNo = static_cast<short>(armNo);
      m_robotCmdParam[ACTION_RESTORE].putPosition = static_cast<short>(fromPos);
      m_robotCmdParam[ACTION_RESTORE].putSlotNo = static_cast<short>(fromSlot);

      return seq + 1;
   }

   ROBOT_CMD_PARAM& CRobotTask::getRobotCmdParam()
   ROBOT_CMD_PARAM& CRobotTask::getRobotCmdParam(int index)
   {
      return m_robotCmdParam;
      ASSERT(ACTION_PICK <= index && index <= ACTION_TRANSFER);
      return m_robotCmdParam[index];
   }

   time_t CRobotTask::getCreateTime()
@@ -134,9 +167,117 @@
      return m_timeFinish;
   }

   int CRobotTask::getArmNo()
   {
      return m_robotCmdParam[ACTION_PICK].armNo;

   }

   ROBOT_TASK_STATE CRobotTask::getState()
   {
      return m_state;
   }

   bool CRobotTask::isPicking()
   {
      return m_state == ROBOT_TASK_STATE::Picking;
   }

   bool CRobotTask::isPlacing()
   {
      return m_state == ROBOT_TASK_STATE::Placing;
   }

   bool CRobotTask::isRestoring()
   {
      return m_state == ROBOT_TASK_STATE::Restoring;
   }

   void CRobotTask::run()
   {
      ASSERT(m_pEFEM);
      m_state = ROBOT_TASK_STATE::Running;

      static int seq = 0;
      m_pEFEM->robotSendTransfer(++seq,
         m_robotCmdParam[ACTION_TRANSFER].armNo,
         m_robotCmdParam[ACTION_TRANSFER].getPosition,
         m_robotCmdParam[ACTION_TRANSFER].putPosition,
         m_robotCmdParam[ACTION_TRANSFER].getSlotNo,
         m_robotCmdParam[ACTION_TRANSFER].putSlotNo,
         [&](int code) -> int {
            if (code == WOK) {
               LOGI(_T("RobotTask已下发到EFEM"));
            }
            else {
               LOGI(_T("RobotTask已下发失败"));
            }

            return 0;
         });
   }

   void CRobotTask::pick()
   {
      ASSERT(m_pEFEM);
      m_state = ROBOT_TASK_STATE::Picking;

      m_pEFEM->robotSendMoveToGet(m_robotCmdParam->sequenceNo,
         m_robotCmdParam[ACTION_PICK].armNo,
         m_robotCmdParam[ACTION_PICK].getPosition,
         m_robotCmdParam[ACTION_PICK].getSlotNo,
         [&](int code) -> int {
            if (code == WOK) {
               LOGI(_T("RobotTask/get已下发到EFEM"));
            }
            else {
               LOGI(_T("RobotTask/get已下发失败"));
            }

            return 0;
         });
   }

   void CRobotTask::place()
   {
      ASSERT(m_pEFEM);
      m_state = ROBOT_TASK_STATE::Placing;

      m_pEFEM->robotSendMoveToPut(m_robotCmdParam->sequenceNo,
         m_robotCmdParam[ACTION_PLACE].armNo,
         m_robotCmdParam[ACTION_PLACE].putPosition,
         m_robotCmdParam[ACTION_PLACE].putSlotNo,
         [&](int code) -> int {
            if (code == WOK) {
               LOGI(_T("RobotTask/put已下发到EFEM"));
            }
            else {
               LOGI(_T("RobotTask/put已下发失败"));
            }

            return 0;
         });
   }

   void CRobotTask::restore()
   {
      ASSERT(m_pEFEM);
      m_state = ROBOT_TASK_STATE::Restoring;

      m_pEFEM->robotSendMoveToPut(m_robotCmdParam->sequenceNo,
         m_robotCmdParam[ACTION_RESTORE].armNo,
         m_robotCmdParam[ACTION_RESTORE].putPosition,
         m_robotCmdParam[ACTION_RESTORE].putSlotNo,
         [&](int code) -> int {
            if (code == WOK) {
               LOGI(_T("RobotTask/restore-put已下发到EFEM"));
            }
            else {
               LOGI(_T("RobotTask/restore-put已下发失败"));
            }

            return 0;
         });
   }

   void CRobotTask::completed()
@@ -156,22 +297,37 @@

   int CRobotTask::getSrcPosition()
   {
      return m_robotCmdParam.getPosition;
      return m_robotCmdParam[ACTION_TRANSFER].getPosition;
   }

   int CRobotTask::getTarPosition()
   {
      return m_robotCmdParam.putPosition;
      return m_robotCmdParam[ACTION_TRANSFER].putPosition;
   }

   int CRobotTask::getSrcSlot()
   {
      return m_robotCmdParam.getSlotNo;
      return m_robotCmdParam[ACTION_TRANSFER].getSlotNo;
   }

   int CRobotTask::getTarSlot()
   {
      return m_robotCmdParam.putSlotNo;
      return m_robotCmdParam[ACTION_TRANSFER].putSlotNo;
   }

   CString CRobotTask::getStateString()
   {
      switch (m_state) {
      case ROBOT_TASK_STATE::Ready:     return _T("Ready");
      case ROBOT_TASK_STATE::Running:   return _T("Running");
      case ROBOT_TASK_STATE::Picking:   return _T("Picking");
      case ROBOT_TASK_STATE::Placing:   return _T("Placing");
      case ROBOT_TASK_STATE::Restoring: return _T("Restoring");
      case ROBOT_TASK_STATE::Error:     return _T("Error");
      case ROBOT_TASK_STATE::Abort:     return _T("Abort");
      case ROBOT_TASK_STATE::Completed: return _T("Completed");
      default:                          return _T("Unknown");
      }
   }

   void CRobotTask::fetchOut()