Merge branch 'clh' into liuyang

mrDarker

2025-08-27 3e0ceaf4e569ea1f57a14de2f6135d1f1a50d080

 SourceCode/Bond/Servo/CRobotTask.cpp

@@ -1,18 +1,88 @@
#include "stdafx.h"
#include "CRobotTask.h"
#include "ToolUnits.h"
#include "Log.h"


namespace SERVO {
   CRobotTask::CRobotTask()
   {
      generateId(m_strId);
      m_timeCreate = CToolUnits::getTimestamp();
      m_state = ROBOT_TASK_STATE::Ready;
      m_timeCreate = CToolUnits::getUnixTimestamp();
      m_timeFetchOut = 0;
      m_timeStored = 0;
      m_timeFinish = 0;
      m_pContext = nullptr;
      m_pEFEM = nullptr;
   }

   CRobotTask::~CRobotTask()
   {
      if (m_pContext != nullptr) {
         m_pContext->release();
         m_pContext = nullptr;
      }
   }

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

      return strOut;
   }

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

      return strOut;
   }

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

      m_pContext = pContext;
      if (m_pContext != nullptr) {
         m_pContext->addRef();
      }
   }

   CContext* CRobotTask::getContext()
   {
      return m_pContext;
   }

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

   std::string& CRobotTask::generateId(std::string& out)
@@ -34,21 +104,52 @@
      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);

      // 转换一下源和目标站号
      int srcPos, srcSlot, tarPos, tarSlot;
      transformPosAndSlot(fromPos, fromSlot, srcPos, srcSlot);
      transformPosAndSlot(toPos, toSlot, tarPos, tarSlot);

      m_robotCmdParam[ACTION_PICK] = {};
      m_robotCmdParam[ACTION_PICK].sequenceNo = static_cast<short>(seq+1);
      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>(srcPos);
      m_robotCmdParam[ACTION_PICK].getSlotNo = static_cast<short>(srcSlot);

      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>(tarPos);
      m_robotCmdParam[ACTION_PLACE].putSlotNo = static_cast<short>(tarSlot);

      m_robotCmdParam[ACTION_RESTORE] = {};
      m_robotCmdParam[ACTION_RESTORE].sequenceNo = static_cast<short>(seq + 3);
      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>(srcPos);
      m_robotCmdParam[ACTION_RESTORE].putSlotNo = static_cast<short>(srcSlot);

      return seq + 3;
   }

   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()
@@ -56,8 +157,261 @@
      return m_timeCreate;
   }

   time_t CRobotTask::getFetchoutTime()
   {
      return m_timeFetchOut;
   }

   time_t CRobotTask::getStoredTime()
   {
      return m_timeStored;
   }

   time_t CRobotTask::getFinishTime()
   {
      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::isPicked()
   {
      return m_state == ROBOT_TASK_STATE::Picked;
   }

   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->robotCmd(m_robotCmdParam[ACTION_PICK],
         [&](int code) -> int {
            if (code == WOK) {
               LOGI(_T("RobotTask/get已下发到EFEM"));
            }
            else {
               LOGI(_T("RobotTask/get已下发失败"));
            }

            return 0;
         });
   }

   void CRobotTask::picked()
   {
      m_state = ROBOT_TASK_STATE::Picked;
   }

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

      m_pEFEM->robotCmd(m_robotCmdParam[ACTION_PLACE],
         [&](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->robotCmd(m_robotCmdParam[ACTION_RESTORE],
         [&](int code) -> int {
            if (code == WOK) {
               LOGI(_T("RobotTask/restore-put已下发到EFEM"));
            }
            else {
               LOGI(_T("RobotTask/restore-put已下发失败"));
            }

            return 0;
         });
   }

   void CRobotTask::resend()
   {
      // 重新下发命令，无非是下发取料或下发放料的命令，根据当前状态来
      if (ROBOT_TASK_STATE::Picking == m_state  || ROBOT_TASK_STATE::Picked == m_state) {
         pick();
      }
      else if (ROBOT_TASK_STATE::Placing == m_state) {
         place();
      }
   }

   void CRobotTask::completed()
   {
      m_state = ROBOT_TASK_STATE::Completed;
   }

   void CRobotTask::restored()
   {
      m_state = ROBOT_TASK_STATE::Restored;
   }

   void CRobotTask::error()
   {
      m_state = ROBOT_TASK_STATE::Error;
   }

   void CRobotTask::abort()
   {
      m_state = ROBOT_TASK_STATE::Abort;
   }

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

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

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

   int CRobotTask::getTarSlot()
   {
      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()
   {
      m_timeFetchOut = CToolUnits::getUnixTimestamp();;
   }

   void CRobotTask::stored()
   {
      m_timeStored = CToolUnits::getUnixTimestamp();;
   }

   void CRobotTask::transformPosAndSlot(int srcPos, int srcSlot, int& tarPos, int& tarSlot)
   {
      switch (srcPos)
      {
      case EQ_ID_LOADPORT1:
      case EQ_ID_LOADPORT2:
      case EQ_ID_LOADPORT3:
      case EQ_ID_LOADPORT4:
         tarPos = srcPos;
         tarSlot = srcSlot;
         break;
      case EQ_ID_ARM_TRAY1:
      case EQ_ID_ARM_TRAY2:
      case EQ_ID_ALIGNER:
      case EQ_ID_FLIPER:
         tarPos = srcPos;
         tarSlot = 1;
         break;
      case EQ_ID_Bonder1:
         if (1 <= srcSlot && srcSlot <= 2) {
            tarPos = 9 + srcSlot - 1;
            tarSlot = 1;
         }
         break;
      case EQ_ID_Bonder2:
         if (1 <= srcSlot && srcSlot <= 2) {
            tarPos = 11 + srcSlot - 1;
            tarSlot = 1;
         }
         break;
      case EQ_ID_VACUUMBAKE:
         if (1 <= srcSlot && srcSlot <= 2) {
            tarPos = 13 + srcSlot - 1;
            tarSlot = 1;
         }
         break;
      case EQ_ID_BAKE_COOLING:
         if (1 <= srcSlot && srcSlot <= 4) {
            tarPos = 15 + srcSlot - 1;
            tarSlot = 1;
         }
         break;
      case EQ_ID_MEASUREMENT:
         tarPos = 19;
         tarSlot = 1;
         break;
      default:
         tarPos = srcPos;
         tarSlot = srcSlot;
         break;
      }
   }
}