Merge branch 'clh' into liuyang

mrDarker

2025-06-26 adf46e4da2f53d171cc5a0e95340d751ebacf763

 SourceCode/Bond/Servo/CPageGraph1.cpp

@@ -7,7 +7,22 @@
#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 } },
   { SERVO::ROBOT_POSITION::Port3,     { SERVO::ROBOT_POSITION::Port3,     0.75f,   0.00f } },
   { SERVO::ROBOT_POSITION::Port4,     { SERVO::ROBOT_POSITION::Port4,     0.60f,   0.00f } },
   { SERVO::ROBOT_POSITION::Aligner,   { SERVO::ROBOT_POSITION::Aligner,   0.40f,   0.00f } },
   { SERVO::ROBOT_POSITION::Fliper,    { SERVO::ROBOT_POSITION::Fliper,    0.25f,    0.00f } },
   { SERVO::ROBOT_POSITION::Bonder1,   { SERVO::ROBOT_POSITION::Bonder1,   0.00f,   0.00f } },
   { SERVO::ROBOT_POSITION::Bonder2,   { SERVO::ROBOT_POSITION::Bonder2,   0.00f,   180.00f } },
   { SERVO::ROBOT_POSITION::Bake,      { SERVO::ROBOT_POSITION::Bake,      0.35f,   180.00f } },
   { SERVO::ROBOT_POSITION::Cooling,   { SERVO::ROBOT_POSITION::Cooling,   0.65f,   180.00f } },
   { SERVO::ROBOT_POSITION::Measurement,{SERVO::ROBOT_POSITION::Measurement,1.00f,  180.00f } },
};

// Image
#define IMAGE_ROBOT            2
@@ -83,8 +98,34 @@
                  BOOL bAlive = pEquipment->isAlive();
                  if (EQ_ID_EFEM == nID) {
                     DeviceStatus status = bAlive ? DeviceStatus::ONLINE : DeviceStatus::OFFLINE;
                     UpdateDeviceStatus(INDICATE_FLIPER, status);
                     UpdateDeviceStatus(INDICATE_ALIGNER, status);
                     UpdateDeviceStatus(INDICATE_LPORT1, status);
                     UpdateDeviceStatus(INDICATE_LPORT2, status);
                     UpdateDeviceStatus(INDICATE_LPORT3, status);
                     UpdateDeviceStatus(INDICATE_LPORT4, status);
                     UpdateDeviceStatus(INDICATE_ROBOT_ARM1, status);
                     UpdateDeviceStatus(INDICATE_ROBOT_ARM2, status);
                  }
                  else if(EQ_ID_Bonder1 == nID) {
                     DeviceStatus status = bAlive ? DeviceStatus::ONLINE : DeviceStatus::OFFLINE;
                     UpdateDeviceStatus(INDICATE_BONDER1, status);
                  }
                  else if (EQ_ID_Bonder2 == nID) {
                     DeviceStatus status = bAlive ? DeviceStatus::ONLINE : DeviceStatus::OFFLINE;
                     UpdateDeviceStatus(INDICATE_BONDER2, status);
                  }
                  else if (EQ_ID_VACUUMBAKE == nID) {
                     DeviceStatus status = bAlive ? DeviceStatus::ONLINE : DeviceStatus::OFFLINE;
                     UpdateDeviceStatus(INDICATE_VACUUM_BAKE, status);
                  }
                  else if (EQ_ID_BAKE_COOLING == nID) {
                     DeviceStatus status = bAlive ? DeviceStatus::ONLINE : DeviceStatus::OFFLINE;
                     UpdateDeviceStatus(INDICATE_BAKE_COOLING, status);
                  }
                  else if (EQ_ID_MEASUREMENT == nID) {
                     DeviceStatus status = bAlive ? DeviceStatus::ONLINE : DeviceStatus::OFFLINE;
                     UpdateDeviceStatus(INDICATE_MEASUREMENT, status);
                  }
               }
            }
@@ -106,6 +147,7 @@
BOOL CPageGraph1::OnInitDialog()
{
   CDialogEx::OnInitDialog();
   InitRxWindows();
   SetTimer(1, 3000, nullptr);


@@ -171,8 +213,6 @@
   m_pGraph->AddIndicateBox(INDICATE_ROBOT_ARM2, 243, 294, 48, RGB(22, 22, 22),
      RGB(255, 127, 39), EQ_BOX_OFFLINE);
   m_pGraph->SetBoxText(INDICATE_ROBOT_ARM2, "6", "Robot");




   // Vacuum bake
@@ -263,6 +303,11 @@
   if (m_hbrBkgnd != nullptr) {
      ::DeleteObject(m_hbrBkgnd);
   }

   if (m_pObserver != nullptr) {
      m_pObserver->unsubscribe();
      m_pObserver = NULL;
   }
}

void CPageGraph1::OnSize(UINT nType, int cx, int cy)
@@ -277,101 +322,97 @@

void CPageGraph1::UpdateRobotPosition(float percentage)
{
   // 限制百分比范围在 [0, 1] 之间
   if (percentage < 0.0f) percentage = 0.0f;
   if (percentage > 1.0f) percentage = 1.0f;

   // 根据百分比计算目标 X 坐标
   int startX = m_pGraph->GetImage(IMAGE_ROBOT)->x;
   auto* pImage = m_pGraph->GetImage(IMAGE_ROBOT);
   if (!pImage) return;

   // 获取当前角度（已通过 RotateRobot 设置）
   float angleDegrees = pImage->angle;
   float radians = angleDegrees * 3.1415926f / 180.0f;

   int startX = pImage->x;
   int endX = static_cast<int>(170 + percentage * (700 - 170));
   int y = 270;
   int cy = y + pImage->bmHeight / 2;

   int arm1Offset = 20;  // 从图片到ARM1的偏移
   int arm2Offset = 73;  // 从图片到ARM2的偏移

   // 计算移动所需的时间
   // 动画时间
   int distance = abs(endX - startX);
   int duration = static_cast<int>((distance / 100.0) * 1000);

   int duration = static_cast<int>((distance / 100.0f) * 1000);
   auto startTime = std::chrono::steady_clock::now();
   auto endTime = startTime + std::chrono::milliseconds(duration);

   // 开始移动，设置标记
   m_bIsRobotMoving = TRUE;

   // 开始平滑移动
   while (std::chrono::steady_clock::now() < endTime) {
      auto currentTime = std::chrono::steady_clock::now();
      float progress = std::chrono::duration<float, std::milli>(currentTime - startTime).count() / duration;
      progress = min(progress, 1.0f);

      // 根据进度计算当前位置
      int currentX = static_cast<int>(startX + progress * (endX - startX));
      m_pGraph->UpdateImageCoordinates(IMAGE_ROBOT, currentX, 270);
      m_pGraph->UpdateIndicateBoxCoordinates(INDICATE_ROBOT_ARM1, currentX + arm1Offset, 294);
      m_pGraph->UpdateIndicateBoxCoordinates(INDICATE_ROBOT_ARM2, currentX + arm2Offset, 294);
      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);

      // 应用所有元素的新坐标
      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();

      // 控制帧率约为 60 FPS
      std::this_thread::sleep_for(std::chrono::milliseconds(16));
   }

   // 确保最后位置精确到目标位置
   m_pGraph->UpdateImageCoordinates(IMAGE_ROBOT, endX, 270);
   m_pGraph->UpdateIndicateBoxCoordinates(INDICATE_ROBOT_ARM1, endX + arm1Offset, 294);
   m_pGraph->UpdateIndicateBoxCoordinates(INDICATE_ROBOT_ARM2, endX + arm2Offset, 294);
   // 最终位置校正
   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);

   // 界面重绘
   m_pGraph->UpdateImageCoordinates(IMAGE_ROBOT, endX, y);
   m_pGraph->UpdateIndicateBoxCoordinates(INDICATE_ROBOT_ARM1, cx + rotatedX1, cy + rotatedY1);
   m_pGraph->UpdateIndicateBoxCoordinates(INDICATE_ROBOT_ARM2, cx + rotatedX2, cy + rotatedY2);

   Invalidate();

   // 动画结束，设置标记
   m_bIsRobotMoving = FALSE;
}

void CPageGraph1::RotateRobot(float angleInDegrees)
{
   // 将角度转换为弧度
   float angleInRadians = static_cast<float>(std::acos(-1)) / 180.0f * angleInDegrees;

   // 获取机器人图片的当前坐标和中心
   // 获取机器人图片
   auto* pImage = m_pGraph->GetImage(IMAGE_ROBOT);
   if (!pImage) return;

   // 更新 Rotate 图片的角度，确保角度保持在 [0, 360) 范围内
   m_pGraph->UpdateImageAngle(IMAGE_ROBOT, static_cast<float>(fmod(pImage->angle + angleInDegrees + 360, 360)));
   // 修正角度为 0~360
   float finalAngle = fmod(angleInDegrees + 360.0f, 360.0f);
   m_pGraph->UpdateImageAngle(IMAGE_ROBOT, finalAngle);

   int cx = pImage->x + pImage->bmWidth / 2;  // 图片中心 X
   int cy = pImage->y + pImage->bmHeight / 2; // 图片中心 Y
   // 计算中心点
   int cx = pImage->x + pImage->bmWidth / 2;
   int cy = pImage->y + pImage->bmHeight / 2;

   // 旋转指示框的坐标
   auto* pRobot1 = m_pGraph->GetIndicateBox(INDICATE_ROBOT_ARM1);
   auto* pRobot2 = m_pGraph->GetIndicateBox(INDICATE_ROBOT_ARM2);
   // 转换角度为弧度
   float radians = angleInDegrees * 3.1415926f / 180.0f;

   if (pRobot1 && pRobot2) {
      int newArmX1 = pImage->x + 20;
      int newArmY1 = 294;
   // 旋转 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 newArmX2 = pImage->x + 73;
      int newArmY2 = 294;
   // 旋转 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);

      if (angleInDegrees != 0.0f) {
         // 计算指示框1的新坐标
         newArmX1 = static_cast<int>(cx + (pRobot1->x - cx) * cos(angleInRadians) - (pRobot1->y - cy) * sin(angleInRadians));
         newArmY1 = static_cast<int>(cy + (pRobot1->x - cx) * sin(angleInRadians) + (pRobot1->y - cy) * cos(angleInRadians));
   // 更新指示框
   m_pGraph->UpdateIndicateBoxCoordinates(INDICATE_ROBOT_ARM1, cx + rotatedX1, cy + rotatedY1);
   m_pGraph->UpdateIndicateBoxCoordinates(INDICATE_ROBOT_ARM2, cx + rotatedX2, cy + rotatedY2);

         // 计算指示框2的新坐标
         newArmX2 = static_cast<int>(cx + (pRobot2->x - cx) * cos(angleInRadians) - (pRobot2->y - cy) * sin(angleInRadians));
         newArmY2 = static_cast<int>(cy + (pRobot2->x - cx) * sin(angleInRadians) + (pRobot2->y - cy) * cos(angleInRadians));
      }

      // 更新指示框的位置
      m_pGraph->UpdateIndicateBoxCoordinates(INDICATE_ROBOT_ARM1, newArmX1, newArmY1);
      m_pGraph->UpdateIndicateBoxCoordinates(INDICATE_ROBOT_ARM2, newArmX2, newArmY2);
   }

   // 强制重绘界面
   Invalidate();
}

@@ -405,16 +446,68 @@
   }
}

void CPageGraph1::MoveRobotToPosition(SERVO::ROBOT_POSITION position)
{
   auto it = g_positionMap.find(position);
   if (it == g_positionMap.end()) {
      TRACE("Invalid robot position: %d\n", static_cast<int>(position));
      return;
   }

   const RobotPositionMapping& mapping = it->second;

   // 平台移动
   UpdateRobotPosition(mapping.percentage);

   // 旋转方向
   RotateRobot(mapping.angle);
}

void CPageGraph1::OnGraphItemClicked(NMHDR* pNMHDR, LRESULT* pResult)
{
   BYSERVOGRAPH_NMHDR* pGraphNmhdr = reinterpret_cast<BYSERVOGRAPH_NMHDR*>(pNMHDR);

   // 移动到指定位置 (测试使用)
   if (pGraphNmhdr->dwData == INDICATE_LPORT1) {
      MoveRobotToPosition(SERVO::ROBOT_POSITION::Port1);
   }
   else if (pGraphNmhdr->dwData == INDICATE_LPORT2) {
      MoveRobotToPosition(SERVO::ROBOT_POSITION::Port2);
   }
   else if (pGraphNmhdr->dwData == INDICATE_LPORT3) {
      MoveRobotToPosition(SERVO::ROBOT_POSITION::Port3);
   }
   else if (pGraphNmhdr->dwData == INDICATE_LPORT4) {
      MoveRobotToPosition(SERVO::ROBOT_POSITION::Port4);
   }
   else if (pGraphNmhdr->dwData == INDICATE_ALIGNER) {
      MoveRobotToPosition(SERVO::ROBOT_POSITION::Aligner);
   }
   else if (pGraphNmhdr->dwData == INDICATE_FLIPER) {
      MoveRobotToPosition(SERVO::ROBOT_POSITION::Fliper);
   }
   else if (pGraphNmhdr->dwData == INDICATE_BONDER1) {
      MoveRobotToPosition(SERVO::ROBOT_POSITION::Bonder1);
   }
   else if (pGraphNmhdr->dwData == INDICATE_BONDER2) {
      MoveRobotToPosition(SERVO::ROBOT_POSITION::Bonder2);
   }
   else if (pGraphNmhdr->dwData == INDICATE_VACUUM_BAKE) {
      MoveRobotToPosition(SERVO::ROBOT_POSITION::Bake);
   }
   else if (pGraphNmhdr->dwData == INDICATE_BAKE_COOLING) {
      MoveRobotToPosition(SERVO::ROBOT_POSITION::Cooling);
   }
   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) {
      //AfxMessageBox(pEquipment->getName().c_str());
      theApp.m_model.notifyPtr(RX_CODE_SELECT_EQUIPMENT, pEquipment);
   }

	
   *pResult = 0;
}

@@ -422,7 +515,29 @@
{
   if (1 == nIDEvent) {
      KillTimer(1);
      InitRxWindows();

      // 更新状态
      {
         SERVO::CEquipment* pEquipment = (SERVO::CEFEM*)theApp.m_model.m_master.getEquipment(EQ_ID_EFEM);
         ASSERT(pEquipment);
         DeviceStatus status = pEquipment->isAlive() ? DeviceStatus::ONLINE : DeviceStatus::OFFLINE;
         UpdateDeviceStatus(INDICATE_ROBOT_ARM1, status);
         UpdateDeviceStatus(INDICATE_ROBOT_ARM2, status);
      }

      {
         SERVO::CEquipment* pEquipment = theApp.m_model.m_master.getEquipment(EQ_ID_Bonder1);
         ASSERT(pEquipment);
         DeviceStatus status = pEquipment->isAlive() ? DeviceStatus::ONLINE : DeviceStatus::OFFLINE;
         UpdateDeviceStatus(INDICATE_BONDER1, status);
      }

      {
         SERVO::CEquipment* pEquipment = theApp.m_model.m_master.getEquipment(EQ_ID_Bonder2);
         ASSERT(pEquipment);
         DeviceStatus status = pEquipment->isAlive() ? DeviceStatus::ONLINE : DeviceStatus::OFFLINE;
         UpdateDeviceStatus(INDICATE_BONDER2, status);
      }
   }

   CDialogEx::OnTimer(nIDEvent);