1.实现RobotCmd的下发，待测；

LAPTOP-SNT8I5JK\Boounion

2025-05-19 0ec075ed33382a0fd69b680c42ab6ed537b1eca7

 SourceCode/Bond/Servo/CEFEM.cpp

@@ -1,5 +1,6 @@
#include "stdafx.h"
#include "CEFEM.h"
#include "Log.h"


#define ADD_EQ_CASSETTE_TRANSFER_STATE_STEP(name,ws,index,psd) {            \
@@ -56,6 +57,73 @@
      if (index < 2) {
         m_pArmTray[index] = pArmTray;
      }
   }

   int CEFEM::robotCmd(ROBOT_CMD_PARAM& robotCmdParam, ONWRITED onWritedBlock/* = nullptr*/)
   {
      SERVO::CEqWriteStep* pStep = (SERVO::CEqWriteStep*)getStepWithName(STEP_EFEM_ROBOT_CMD);
      if (pStep == nullptr) {
         return -1;
      }

      LOGI("<CEFEM-%s>准备发送RobotCmd<%d>", m_strName.c_str(), (int)robotCmdParam.rcmd);
      if (onWritedBlock != nullptr) {
         pStep->writeDataEx((const char*)&robotCmdParam, ROBOT_CMD_PARAM_SIZE, onWritedBlock);
      }
      else {
         pStep->writeDataEx((const char*)&robotCmdParam, ROBOT_CMD_PARAM_SIZE, [&](int code) -> int {
            if (code == WOK) {
               LOGI("<CEFEM-%s>发送RobotCmd成功.", m_strName.c_str());
            }
            else {
               LOGI("<CEFEM-%s>发送RobotCmd失败，code:%d", m_strName.c_str(), code);
            }

            return 0;
            });
      }

      return 0;
   }

   int CEFEM::robotCmds(ROBOT_CMD_PARAM* robotCmdParam, unsigned int count, ONWRITED onWritedBlock/* = nullptr*/)
   {
      SERVO::CEqWriteStep* pStep = (SERVO::CEqWriteStep*)getStepWithName(STEP_EFEM_ROBOT_CMD);
      if (pStep == nullptr) {
         return -1;
      }


      // 平铺到内存中，如果ROBOT_CMD_PARAM不是4的整数倍，只能一个一个复制
      std::string strLog = "准备发送RobotCmds<";
      char szBuffer[ROBOT_CMD_PARAM_SIZE * 4];
      for (int i = 0; i < min(4, count); i++) {
         memcpy(&szBuffer[i * ROBOT_CMD_PARAM_SIZE], (robotCmdParam + i), ROBOT_CMD_PARAM_SIZE);
         strLog += std::to_string((robotCmdParam + i)->rcmd);
         strLog += ",";
         LOGI("%d", (robotCmdParam + 1)->rcmd);
      }
      strLog += ">.";
      LOGI("<CEFEM-%s>%s", m_strName.c_str(), strLog.c_str());


      if (onWritedBlock != nullptr) {
         pStep->writeDataEx(szBuffer, ROBOT_CMD_PARAM_SIZE * min(4, count), onWritedBlock);
      }
      else {
         pStep->writeDataEx(szBuffer, ROBOT_CMD_PARAM_SIZE * min(4, count), [&](int code) -> int {
            if (code == WOK) {
               LOGI("<CEquipment-%s>发送RobotCmdS成功.", m_strName.c_str());
            }
            else {
               LOGI("<CEquipment-%s>发送RobotCmds失败，code:%d", m_strName.c_str(), code);
            }

            return 0;
            });
      }

      return 0;
   }

   void CEFEM::init()
@@ -375,6 +443,17 @@
            delete pStep;
         }
      }

      {
         // Robot CMD
         CEqWriteStep* pStep = new CEqWriteStep();
         pStep->setName(STEP_EFEM_ROBOT_CMD);
         pStep->setWriteSignalDev(0x14D);
         pStep->setDataDev(0x90a);
         if (addStep(STEP_ID_ROBOT_CMD_REPLY, pStep) != 0) {
            delete pStep;
         }
      }
   }

   int CEFEM::onStepEvent(CStep* pStep, int code)