1. 防止定时器重复刷新手臂的状态 2. 手臂显示的位置通过配置文件读取

mrDarker

2025-06-27 74401edd0fb47294538d7253ad15e6a8d3d1fc5a

 SourceCode/Bond/Servo/CPageGraph1.cpp

@@ -7,9 +7,6 @@
#include "afxdialogex.h"
#include "Common.h"

const POINT g_arm1Offset = { -30, -45 }; // ARM1 从中心向左47, 向上33
const POINT g_arm2Offset = { 27, -45 };    // ARM2 从中心向右10, 向上33

const std::map<SERVO::ROBOT_POSITION, RobotPositionMapping> g_positionMap = {
   { SERVO::ROBOT_POSITION::Port1,     { SERVO::ROBOT_POSITION::Port1,     1.00f,   0.00f } },
   { SERVO::ROBOT_POSITION::Port2,     { SERVO::ROBOT_POSITION::Port2,     0.90f,   0.00f } },
@@ -57,6 +54,15 @@
   m_bIsRobotMoving = FALSE;
   m_crBkgnd = PAGE_GRPAH1_BACKGROUND_COLOR;
   m_hbrBkgnd = nullptr;
   m_lastRobotPosition = SERVO::ROBOT_POSITION::Port1;
   m_lastArmState[0] = FALSE;
   m_lastArmState[1] = FALSE;

   m_arm1Offset = LoadArmOffset("ARM1");
   m_arm2Offset = LoadArmOffset("ARM2");

   //m_arm1Offset = { -30, -45 }; // ARM1 从中心向左47, 向上33
   //m_arm2Offset = { 27, -45 };    // ARM2 从中心向右10, 向上33
}

CPageGraph1::~CPageGraph1()
@@ -81,6 +87,15 @@

// CPageGraph1 消息处理程序

std::string CPageGraph1::GetConfigPath()
{
   char path[MAX_PATH];
   GetModuleFileNameA(NULL, path, MAX_PATH);
   std::string exePath(path);
   std::string configDir = exePath.substr(0, exePath.find_last_of("\\/")) + "\\Config";
   CreateDirectoryA(configDir.c_str(), NULL);
   return configDir + "\\robot_offset.ini";
}

void CPageGraph1::InitRxWindows()
{
@@ -319,6 +334,9 @@
      m_pObserver->unsubscribe();
      m_pObserver = NULL;
   }

   SaveArmOffset("ARM1", m_arm1Offset);
   SaveArmOffset("ARM2", m_arm2Offset);
}

void CPageGraph1::OnSize(UINT nType, int cx, int cy)
@@ -365,10 +383,10 @@
      int cx = currentX + pImage->bmWidth / 2;

      // 旋转后的偏移
      int rotatedX1 = static_cast<int>(cos(radians) * g_arm1Offset.x - sin(radians) * g_arm1Offset.y);
      int rotatedY1 = static_cast<int>(sin(radians) * g_arm1Offset.x + cos(radians) * g_arm1Offset.y);
      int rotatedX2 = static_cast<int>(cos(radians) * g_arm2Offset.x - sin(radians) * g_arm2Offset.y);
      int rotatedY2 = static_cast<int>(sin(radians) * g_arm2Offset.x + cos(radians) * g_arm2Offset.y);
      int rotatedX1 = static_cast<int>(cos(radians) * m_arm1Offset.x - sin(radians) * m_arm1Offset.y);
      int rotatedY1 = static_cast<int>(sin(radians) * m_arm1Offset.x + cos(radians) * m_arm1Offset.y);
      int rotatedX2 = static_cast<int>(cos(radians) * m_arm2Offset.x - sin(radians) * m_arm2Offset.y);
      int rotatedY2 = static_cast<int>(sin(radians) * m_arm2Offset.x + cos(radians) * m_arm2Offset.y);

      // 应用所有元素的新坐标
      m_pGraph->UpdateImageCoordinates(IMAGE_ROBOT, currentX, y);
@@ -382,10 +400,10 @@

   // 最终位置校正
   int cx = endX + pImage->bmWidth / 2;
   int rotatedX1 = static_cast<int>(cos(radians) * g_arm1Offset.x - sin(radians) * g_arm1Offset.y);
   int rotatedY1 = static_cast<int>(sin(radians) * g_arm1Offset.x + cos(radians) * g_arm1Offset.y);
   int rotatedX2 = static_cast<int>(cos(radians) * g_arm2Offset.x - sin(radians) * g_arm2Offset.y);
   int rotatedY2 = static_cast<int>(sin(radians) * g_arm2Offset.x + cos(radians) * g_arm2Offset.y);
   int rotatedX1 = static_cast<int>(cos(radians) * m_arm1Offset.x - sin(radians) * m_arm1Offset.y);
   int rotatedY1 = static_cast<int>(sin(radians) * m_arm1Offset.x + cos(radians) * m_arm1Offset.y);
   int rotatedX2 = static_cast<int>(cos(radians) * m_arm2Offset.x - sin(radians) * m_arm2Offset.y);
   int rotatedY2 = static_cast<int>(sin(radians) * m_arm2Offset.x + cos(radians) * m_arm2Offset.y);

   m_pGraph->UpdateImageCoordinates(IMAGE_ROBOT, endX, y);
   m_pGraph->UpdateIndicateBoxCoordinates(INDICATE_ROBOT_ARM1, cx + rotatedX1, cy + rotatedY1);
@@ -413,12 +431,12 @@
   float radians = angleInDegrees * 3.1415926f / 180.0f;

   // 旋转 offset1
   int rotatedX1 = static_cast<int>(cos(radians) * g_arm1Offset.x - sin(radians) * g_arm1Offset.y);
   int rotatedY1 = static_cast<int>(sin(radians) * g_arm1Offset.x + cos(radians) * g_arm1Offset.y);
   int rotatedX1 = static_cast<int>(cos(radians) * m_arm1Offset.x - sin(radians) * m_arm1Offset.y);
   int rotatedY1 = static_cast<int>(sin(radians) * m_arm1Offset.x + cos(radians) * m_arm1Offset.y);

   // 旋转 offset2
   int rotatedX2 = static_cast<int>(cos(radians) * g_arm2Offset.x - sin(radians) * g_arm2Offset.y);
   int rotatedY2 = static_cast<int>(sin(radians) * g_arm2Offset.x + cos(radians) * g_arm2Offset.y);
   int rotatedX2 = static_cast<int>(cos(radians) * m_arm2Offset.x - sin(radians) * m_arm2Offset.y);
   int rotatedY2 = static_cast<int>(sin(radians) * m_arm2Offset.x + cos(radians) * m_arm2Offset.y);

   // 更新指示框
   m_pGraph->UpdateIndicateBoxCoordinates(INDICATE_ROBOT_ARM1, cx + rotatedX1, cy + rotatedY1);
@@ -476,6 +494,27 @@
   m_lastRobotPosition = position;
}

POINT CPageGraph1::LoadArmOffset(const std::string& armName)
{
   std::string iniPath = GetConfigPath();
   POINT pt;
   pt.x = GetPrivateProfileIntA("Offsets", (armName + "_X").c_str(), 0, iniPath.c_str());
   pt.y = GetPrivateProfileIntA("Offsets", (armName + "_Y").c_str(), 0, iniPath.c_str());
   return pt;
}

void CPageGraph1::SaveArmOffset(const std::string& armName, const POINT& pt)
{
   std::string iniPath = GetConfigPath();
   char buf[16];

   sprintf_s(buf, "%d", pt.x);
   WritePrivateProfileStringA("Offsets", (armName + "_X").c_str(), buf, iniPath.c_str());

   sprintf_s(buf, "%d", pt.y);
   WritePrivateProfileStringA("Offsets", (armName + "_Y").c_str(), buf, iniPath.c_str());
}

void CPageGraph1::OnGraphItemClicked(NMHDR* pNMHDR, LRESULT* pResult)
{
   BYSERVOGRAPH_NMHDR* pGraphNmhdr = reinterpret_cast<BYSERVOGRAPH_NMHDR*>(pNMHDR);
@@ -513,12 +552,6 @@
   }
   else if (pGraphNmhdr->dwData == INDICATE_MEASUREMENT) {
      MoveRobotToPosition(SERVO::ROBOT_POSITION::Measurement);
   }

   CString s; s.Format(_T("OnGraphItemClicked %d"), pGraphNmhdr->dwData);
   SERVO::CEquipment* pEquipment = (SERVO::CEquipment*)m_pGraph->GetIndicateBoxData(pGraphNmhdr->dwData);
   if (pEquipment != nullptr) {
      theApp.m_model.notifyPtr(RX_CODE_SELECT_EQUIPMENT, pEquipment);
   }
   
   *pResult = 0;
@@ -587,10 +620,19 @@

      // 如果设备在线，那么更新 ARM 状态
      SERVO::RMDATA& robotData = pEFEM->getRobotMonitoringData();
      DeviceStatus arm1Status = robotData.armState[0] ? DeviceStatus::OCCUPIED : DeviceStatus::ONLINE;
      DeviceStatus arm2Status = robotData.armState[1] ? DeviceStatus::OCCUPIED : DeviceStatus::ONLINE;
      UpdateDeviceStatus(INDICATE_ROBOT_ARM1, arm1Status);
      UpdateDeviceStatus(INDICATE_ROBOT_ARM2, arm2Status);
      if (m_lastArmState[0] != robotData.armState[0]) {
         m_lastArmState[0] = robotData.armState[0];
         DeviceStatus arm1Status;
         arm1Status = robotData.armState[0] ? DeviceStatus::OCCUPIED : DeviceStatus::ONLINE;
         UpdateDeviceStatus(INDICATE_ROBOT_ARM1, arm1Status);
      }

      if (m_lastArmState[1] != robotData.armState[1]) {
         m_lastArmState[1] = robotData.armState[1];
         DeviceStatus arm2Status;
         arm2Status = robotData.armState[1] ? DeviceStatus::OCCUPIED : DeviceStatus::ONLINE;
         UpdateDeviceStatus(INDICATE_ROBOT_ARM2, arm2Status);
      }

      // 位置信息状态显示
      if (robotData.position != m_lastRobotPosition) {