Merge branch 'liuyang' into clh

LAPTOP-SNT8I5JK\Boounion

2025-06-30 e979a31c4e7c1d54703230ef3335d6e929442493

 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()
{
@@ -272,7 +287,10 @@
   }

   m_pGraph->UpdateIndicateBox1Colors(id, newBackgroundColor, newFrameColor1, newFrameColor2);
   m_pGraph->Invalidata();

   // 刷新界面
   Invalidate();
   UpdateWindow();
}

BOOL CPageGraph1::OnEraseBkgnd(CDC* pDC)
@@ -316,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)
@@ -362,25 +383,27 @@
      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);
      m_pGraph->UpdateIndicateBoxCoordinates(INDICATE_ROBOT_ARM1, cx + rotatedX1, cy + rotatedY1);
      m_pGraph->UpdateIndicateBoxCoordinates(INDICATE_ROBOT_ARM2, cx + rotatedX2, cy + rotatedY2);

      Invalidate();
      UpdateWindow();
      std::this_thread::sleep_for(std::chrono::milliseconds(16));
   }

   // 最终位置校正
   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);
@@ -408,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);
@@ -471,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);
@@ -508,12 +552,6 @@
   }
   else if (pGraphNmhdr->dwData == INDICATE_MEASUREMENT) {
      MoveRobotToPosition(SERVO::ROBOT_POSITION::Measurement);
   }

   SERVO::CEquipment* pEquipment = (SERVO::CEquipment*)m_pGraph->GetIndicateBoxData(pGraphNmhdr->dwData);
   if (pEquipment != nullptr) {
      // 暂时禁用展开属性页的功能
      // theApp.m_model.notifyPtr(RX_CODE_SELECT_EQUIPMENT, pEquipment);
   }
   
   *pResult = 0;
@@ -582,17 +620,24 @@

      // 如果设备在线，那么更新 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) {
         MoveRobotToPosition(robotData.position);
      }

      m_pGraph->Invalidata();
   }

   CDialogEx::OnTimer(nIDEvent);