Merge branch 'clh' into liuyang

mrDarker

6 天以前 829fe6c6bc33d53fda9c31fd45a37e1df87befff

 SourceCode/Bond/Servo/CPageGraph1.cpp

@@ -41,6 +41,7 @@
// 定时器
#define TIMER_ID_DEVICE_STATUS      1   // 用于初始化设备状态
#define TIMER_ID_ROBOT_STATUS       2   // 用于周期刷新机器人位置/臂状态
#define TIMER_ID_ROBOT_ANIMATION   3   // 

// CPageGraph1 对话框

@@ -49,17 +50,29 @@
CPageGraph1::CPageGraph1(CWnd* pParent /*=nullptr*/)
   : CDialogEx(IDD_PAGE_GRAPH1, pParent)
{
   m_pGraph = nullptr;
   m_pObserver = nullptr;
   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_pGraph = nullptr;                                 // 图形绘图对象
   m_pObserver = nullptr;                              // 观察者对象（可能是事件观察者）
   m_crBkgnd = PAGE_GRPAH1_BACKGROUND_COLOR;           // 背景颜色
   m_hbrBkgnd = nullptr;                               // 背景刷句柄

   m_arm1Offset = LoadArmOffset("ARM1");
   m_arm2Offset = LoadArmOffset("ARM2");
   // ===== 机器人动画状态初始化 =====
   m_bIsRobotMoving = FALSE;                           // 当前是否正在动画移动
   m_nRobotMoveStartX = 0;                             // 动画起始 X 坐标
   m_nRobotMoveEndX = 0;                               // 动画目标 X 坐标
   m_nRobotMoveSteps = 30;                             // 动画总步数（动画速度控制）
   m_nRobotMoveCurrentStep = 0;                        // 当前动画步数
   m_nRobotMoveStartAngle = 0.0f;                      // 动画起始角度
   m_nRobotMoveEndAngle = 0.0f;                        // 动画目标角度

   // ===== 机器人上一次状态初始化 =====
   m_lastRobotPosition = SERVO::ROBOT_POSITION::Port1; // 上次机器人位置（默认 Port1）
   m_lastArmState[0] = FALSE;                          // 上次机械臂1 状态（未占用）
   m_lastArmState[1] = FALSE;                          // 上次机械臂2 状态（未占用）

   // ===== 机械臂相对偏移量初始化（从配置中加载） =====
   m_arm1Offset = LoadArmOffset("ARM1");               // 加载机械臂1偏移
   m_arm2Offset = LoadArmOffset("ARM2");               // 加载机械臂2偏移

   //m_arm1Offset = { -30, -45 }; // ARM1 从中心向左47, 向上33
   //m_arm2Offset = { 27, -45 };    // ARM2 从中心向右10, 向上33
@@ -167,7 +180,7 @@
{
   CDialogEx::OnInitDialog();
   InitRxWindows();
   SetTimer(TIMER_ID_DEVICE_STATUS, 3000, nullptr);
   SetTimer(TIMER_ID_DEVICE_STATUS, 800, nullptr);
   SetTimer(TIMER_ID_ROBOT_STATUS, 1000, nullptr); // 每 1000ms 更新一次状态

   // 图示
@@ -183,73 +196,73 @@
   // Bonder
   m_pGraph->AddIndicateBox(INDICATE_BONDER1, 220, 172, 48, RGB(22, 22, 22),
      RGB(255, 127, 39), EQ_BOX_OFFLINE);
   m_pGraph->SetBoxText(INDICATE_BONDER1, "10", "Bonder 1");
   m_pGraph->SetBoxText(INDICATE_BONDER1, "", "Bonder 1");
   m_pGraph->AddIndicateBox(INDICATE_BONDER2, 220, 516, 48, RGB(22, 22, 22),
      RGB(255, 127, 39), EQ_BOX_OFFLINE);
   m_pGraph->SetBoxText(INDICATE_BONDER2, "11", "Bonder 2");
   m_pGraph->SetBoxText(INDICATE_BONDER2, "", "Bonder 2");


   // 翻转
   m_pGraph->AddIndicateBox(INDICATE_FLIPER, 338, 172, 48, RGB(22, 22, 22),
      RGB(255, 127, 39), EQ_BOX_OFFLINE);
   m_pGraph->SetBoxText(INDICATE_FLIPER, "8", "Fliper");
   m_pGraph->SetBoxText(INDICATE_FLIPER, "", "Fliper");


   // 对位
   m_pGraph->AddIndicateBox(INDICATE_ALIGNER, 428, 172, 48, RGB(22, 22, 22),
      RGB(255, 127, 39), EQ_BOX_OFFLINE);
   m_pGraph->SetBoxText(INDICATE_ALIGNER, "7", "Aligner");
   m_pGraph->SetBoxText(INDICATE_ALIGNER, "", "Aligner");


   // Load port 4
   m_pGraph->AddIndicateBox(INDICATE_LPORT4, 518, 172, 48, RGB(22, 22, 22),
      RGB(255, 127, 39), EQ_BOX_OFFLINE);
   m_pGraph->SetBoxText(INDICATE_LPORT4, "4", "LPort4");
   m_pGraph->SetBoxText(INDICATE_LPORT4, "", "LPort4");


   // Load port 3
   m_pGraph->AddIndicateBox(INDICATE_LPORT3, 606, 172, 48, RGB(22, 22, 22),
      RGB(255, 127, 39), EQ_BOX_OFFLINE);
   m_pGraph->SetBoxText(INDICATE_LPORT3, "3", "LPort3");
   m_pGraph->SetBoxText(INDICATE_LPORT3, "", "LPort3");


   // Load port 2
   m_pGraph->AddIndicateBox(INDICATE_LPORT2, 690, 172, 48, RGB(22, 22, 22),
      RGB(255, 127, 39), EQ_BOX_OFFLINE);
   m_pGraph->SetBoxText(INDICATE_LPORT2, "2", "LPort2");
   m_pGraph->SetBoxText(INDICATE_LPORT2, "", "LPort2");


   // Load port 1
   m_pGraph->AddIndicateBox(INDICATE_LPORT1, 774, 172, 48, RGB(22, 22, 22),
      RGB(255, 127, 39), EQ_BOX_OFFLINE);
   m_pGraph->SetBoxText(INDICATE_LPORT1, "1", "LPort1");
   m_pGraph->SetBoxText(INDICATE_LPORT1, "", "LPort1");


   // Robot
   m_pGraph->AddIndicateBox(INDICATE_ROBOT_ARM1, 190, 294, 48, RGB(22, 22, 22),
      RGB(255, 127, 39), EQ_BOX_OFFLINE);
   m_pGraph->SetBoxText(INDICATE_ROBOT_ARM1, "5", "Robot");
   m_pGraph->SetBoxText(INDICATE_ROBOT_ARM1, "", "Robot");
   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");
   m_pGraph->SetBoxText(INDICATE_ROBOT_ARM2, "", "Robot");


   // Vacuum bake
   m_pGraph->AddIndicateBox(INDICATE_VACUUM_BAKE, 396, 516, 48, RGB(22, 22, 22),
      RGB(255, 127, 39), EQ_BOX_OFFLINE);
   m_pGraph->SetBoxText(INDICATE_VACUUM_BAKE, "9", "Vacuum bake");
   m_pGraph->SetBoxText(INDICATE_VACUUM_BAKE, "", "Vacuum bake");


   // Bake cooling
   m_pGraph->AddIndicateBox(INDICATE_BAKE_COOLING, 566, 516, 48, RGB(22, 22, 22),
      RGB(255, 127, 39), EQ_BOX_OFFLINE);
   m_pGraph->SetBoxText(INDICATE_BAKE_COOLING, "12", "Bake cooling");
   m_pGraph->SetBoxText(INDICATE_BAKE_COOLING, "", "Bake cooling");


   // 精度检
   m_pGraph->AddIndicateBox(INDICATE_MEASUREMENT, 737, 516, 48, RGB(22, 22, 22),
      RGB(255, 127, 39), EQ_BOX_OFFLINE);
   m_pGraph->SetBoxText(INDICATE_MEASUREMENT, "13", "Measurement");
   m_pGraph->SetBoxText(INDICATE_MEASUREMENT, "", "Measurement");


   return TRUE;  // return TRUE unless you set the focus to a control
@@ -494,6 +507,32 @@
   m_lastRobotPosition = position;
}

void CPageGraph1::StartRobotMoveToPosition(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;

   auto* pImage = m_pGraph->GetImage(IMAGE_ROBOT);
   if (!pImage) return;

   m_nRobotMoveStartX = pImage->x;
   m_nRobotMoveEndX = static_cast<int>(170 + mapping.percentage * (700 - 170));

   m_nRobotMoveStartAngle = pImage->angle;     // 起始角度（当前角度）
   m_nRobotMoveEndAngle = mapping.angle;       // 目标角度

   m_nRobotMoveCurrentStep = 0;
   m_targetRobotPosition = position;
   m_bIsRobotMoving = TRUE;

   SetTimer(TIMER_ID_ROBOT_ANIMATION, 16, nullptr);
}

POINT CPageGraph1::LoadArmOffset(const std::string& armName)
{
   std::string iniPath = GetConfigPath();
@@ -521,37 +560,7 @@

   // 移动到指定位置 (测试使用)
   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);

   }
   
   *pResult = 0;
@@ -636,9 +645,50 @@

      // 位置信息状态显示
      if (robotData.position != m_lastRobotPosition) {
         MoveRobotToPosition(robotData.position);
         StartRobotMoveToPosition(robotData.position);
      }
   }
   else if (nIDEvent == TIMER_ID_ROBOT_ANIMATION) {
      if (!m_bIsRobotMoving) {
         KillTimer(TIMER_ID_ROBOT_ANIMATION);
         return;
      }

      m_nRobotMoveCurrentStep++;

      float progress = static_cast<float>(m_nRobotMoveCurrentStep) / m_nRobotMoveSteps;
      if (progress >= 1.0f) {
         progress = 1.0f;
         m_bIsRobotMoving = FALSE;
         KillTimer(TIMER_ID_ROBOT_ANIMATION);
         m_lastRobotPosition = m_targetRobotPosition;
      }

      // 平滑计算位置
      int currentX = static_cast<int>(m_nRobotMoveStartX + progress * (m_nRobotMoveEndX - m_nRobotMoveStartX));
      auto* pImage = m_pGraph->GetImage(IMAGE_ROBOT);
      if (!pImage) return;

      int cx = currentX + pImage->bmWidth / 2;
      int y = 270;
      int cy = y + pImage->bmHeight / 2;

      // 平滑计算角度
      float currentAngle = m_nRobotMoveStartAngle + progress * (m_nRobotMoveEndAngle - m_nRobotMoveStartAngle);
      float radians = currentAngle * 3.1415926f / 180.0f;

      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);
      m_pGraph->UpdateImageAngle(IMAGE_ROBOT, currentAngle);

      Invalidate();
   }

   CDialogEx::OnTimer(nIDEvent);
}