auto

.gitignore

@@ -0,0 +1,176 @@
+# Created by https://www.toptal.com/developers/gitignore/api/python
+# Edit at https://www.toptal.com/developers/gitignore?templates=python
+
+### Python ###
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# poetry
+#   Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
+#   This is especially recommended for binary packages to ensure reproducibility, and is more
+#   commonly ignored for libraries.
+#   https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
+#poetry.lock
+
+# pdm
+#   Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
+#pdm.lock
+#   pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
+#   in version control.
+#   https://pdm.fming.dev/#use-with-ide
+.pdm.toml
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
+
+# PyCharm
+#  JetBrains specific template is maintained in a separate JetBrains.gitignore that can
+#  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
+#  and can be added to the global gitignore or merged into this file.  For a more nuclear
+#  option (not recommended) you can uncomment the following to ignore the entire idea folder.
+#.idea/
+
+### Python Patch ###
+# Poetry local configuration file - https://python-poetry.org/docs/configuration/#local-configuration
+poetry.toml
+
+# ruff
+.ruff_cache/
+
+# LSP config files
+pyrightconfig.json
+
+# End of https://www.toptal.com/developers/gitignore/api/python

game_plat.py

@@ -3,7 +3,7 @@ import math
 import numpy as np
 import random
 from utils.cv_marker import cap_and_mark
-from utils.stack_exe import Stackbot
+# from utils.stack_exe import Stackbot
 import cv2
 
 
@@ -79,10 +79,13 @@ class Tower:
         ])
         self.robot_to_sim_matrix = np.linalg.inv(self.sim_to_robot_matrix)
 
-        self.stack_bot = Stackbot()
+        # self.stack_bot = Stackbot()
 
         self.is_upper = False
 
+        self.ideal_positions = self.get_ideal_positions()
+        self.position_tolerance = 20  # 允许的位置偏差（像素）
+        self.angle_tolerance = 15  # 允许的角度偏差（度）
 
     def sim_to_robot_coords(self, x, y, angle):
         sim_coords = np.array([x, y, 0, 1])
@@ -106,7 +109,7 @@ class Tower:
             angle (float): 机器人的角度
 
         返回值：
-            bool：如果方块添加成功，返回 True，否则返回 False
+            bool：如果方块添加成，返回 True，否则返回 False
 
         注意：这个函数需要先调用 robot_to_sim_coords 来将机器人坐标转换为模拟坐标，并且需要一个名为 Block 的类来创建方块对象
         """
@@ -121,21 +124,27 @@ class Tower:
 
 
     def add_block(self, block):
-        if self.current_layer == 5:
+        self.add_block_from_sim(block)
-            self.is_upper = True
+        return True
-            print("is upper")
+        # if self.current_layer == 5:
+        #     self.is_upper = True
+        #     print("is upper")
 
-        if not self.check_interference(block):
+        # if not self.check_interference(block):
-            self.blocks.append(block)
+        #     self.blocks.append(block)
-            self.stack_bot.pick_stack()
+        #     self.stack_bot.pick_stack()
-            robo_xyz = self.sim_to_robot_coords(block.x, block.y, block.angle)
+        #     robo_xyz = self.sim_to_robot_coords(block.x, block.y, block.angle)
-            print(block.x, block.y, block.angle)
+        #     print(block.x, block.y, block.angle)
-            if self.is_upper:
+        #     if self.is_upper:
-                self.stack_bot.place_stack(robo_xyz[0], robo_xyz[1], robo_xyz[2], self.current_layer - 5)
+        #         self.stack_bot.place_stack(robo_xyz[0], robo_xyz[1], robo_xyz[2], self.current_layer - 5)
-            else:
+        #     else:
-                self.stack_bot.place_stack(robo_xyz[0], robo_xyz[1], robo_xyz[2], self.current_layer)
+        #         self.stack_bot.place_stack(robo_xyz[0], robo_xyz[1], robo_xyz[2], self.current_layer)
-            return True
+        #     return True
-        return False
+        # return False
+    
+    def add_block_from_sim(self, block):
+        self.blocks.append(block)
+        return True
 
     def draw(self, surface):
         for block in self.blocks:
@@ -252,49 +261,85 @@ class Tower:
             self.layer_centroids.append(centroid)
         self.current_layer += 1
 
+    def get_ideal_positions(self):
+        # 定义中心原点
+        center_x, center_y = 400, 450
+
+        # 定义相对坐标和角度
+        relative_positions = [
+            [(0, 40, 0), (0, -40, 0)],      # 第一层
+            [(-40, 0, 90), (40, 0, 90)],    # 第二层
+            [(0, 40, 0), (0, -40, 0)],      # 第三层
+            [(-40, 0, 90), (40, 0, 90)],    # 第四层
+            [(0, 40, 0), (0, -40, 0)],      # 第五层
+            # ... 可以继续添加更多层 ...
+        ]
+
+        # 转换为绝对坐标
+        absolute_positions = []
+        for layer in relative_positions:
+            layer_positions = []
+            for rel_x, rel_y, angle in layer:
+                abs_x = center_x + rel_x
+                abs_y = center_y + rel_y
+                layer_positions.append((abs_x, abs_y, angle))
+            absolute_positions.append(layer_positions)
+
+        return absolute_positions
+
     def auto_place_block(self):
-        if self.current_layer == 0:
+        if self.current_layer >= len(self.ideal_positions):
-            # 对于第一层，直接在中心放置
+            print("塔已经完成")
-            return Block(WIDTH // 2, HEIGHT // 2, 100, 20, 0, self.current_layer)
+            return None
 
-        base_centroid = self.layer_centroids[0] if self.layer_centroids else (WIDTH // 2, HEIGHT // 2)
+        possible_positions = self.generate_possible_positions()
-        current_centroid = self.calculate_current_layer_centroid() or base_centroid
+        best_position = self.find_best_position(possible_positions)
 
-        # 计算当前层的总质量（假设质量与宽度成正比）
+        if best_position:
-        total_mass = sum(block.width for block in self.blocks if block.layer == self.current_layer)
+            x, y, angle = best_position
+            print(f"选择的最佳位置: ({x}, {y}) 角度为 {angle}")  # 添加这行来调试
+            return Block(x, y, 100, 20, angle, self.current_layer)
+        else:
+            print("无法找到合适的放置位置")
+            return None
 
-        # 假设新积木块的质量与宽度成正比
+    def generate_possible_positions(self):
-        new_block_mass = 100  # 新积木块的宽度
+        ideal_positions = self.ideal_positions[self.current_layer]
+        possible_positions = []
 
-        # 计算理想位置
+        for ideal_x, ideal_y, ideal_angle in ideal_positions:
-        ideal_x = (base_centroid[0] * (total_mass + new_block_mass) - current_centroid[0] * total_mass) / new_block_mass
+            for dx in range(-self.position_tolerance, self.position_tolerance + 1, 2):
-        ideal_y = (base_centroid[1] * (total_mass + new_block_mass) - current_centroid[1] * total_mass) / new_block_mass
+                for dy in range(-self.position_tolerance, self.position_tolerance + 1, 2):
+                    for dangle in range(-self.angle_tolerance, self.angle_tolerance + 1, 5):
+                        x = ideal_x + dx
+                        y = ideal_y + dy
+                        angle = (ideal_angle + dangle) % 360
+                        possible_positions.append((x, y, angle))
 
-        # 计算木块中心到塔重心的角度
+        print(f"生成的可能位置数量: {len(possible_positions)}")
-        dx = base_centroid[0] - ideal_x
+        return possible_positions
-        dy = base_centroid[1] - ideal_y
-        center_to_centroid_angle = math.degrees(math.atan2(dy, dx))
 
-        # 计算垂直于中心线的理想角度
+    def find_best_position(self, possible_positions):
-        ideal_angle = (center_to_centroid_angle + 90) % 180 + 90
+        best_position = None
+        min_distance = float('inf')
 
-        # 步骤 2: 寻找最佳角度
+        for x, y, angle in possible_positions:
-        best_block = None
+            new_block = Block(x, y, 100, 20, angle, self.current_layer)
-        min_angle_diff = float('inf')
             
-        for angle in range(0, 180, 15):  # 每15度尝试一次
+            if self.has_support(new_block) and not self.check_interference(new_block):
-            test_block = Block(ideal_x, ideal_y, 100, 20, angle, self.current_layer)
+                temp_tower = Tower()
+                temp_tower.blocks = self.blocks.copy()
+                temp_tower.add_block(new_block)
+                if temp_tower.check_stability():
+                    ideal_x, ideal_y, ideal_angle = self.ideal_positions[self.current_layer][len(self.blocks) % 2]
+                    distance = np.sqrt((x - ideal_x)**2 + (y - ideal_y)**2) + abs(angle - ideal_angle)
                     
-            if not self.check_interference(test_block):
+                    if distance < min_distance:
-                # 检查是否有支撑
+                        min_distance = distance
-                if self.has_support(test_block):
+                        best_position = (x, y, angle)
-                    # 计算与理想角度的差异
-                    angle_diff = min((angle - ideal_angle) % 180, (ideal_angle - angle) % 180)
-                    if angle_diff < min_angle_diff:
-                        min_angle_diff = angle_diff
-                        best_block = test_block
 
-        return best_block
+        print(f"选择的最佳位置: {best_position}，距离理想位置的距离: {min_distance}")
+        return best_position
 
     def has_support(self, block):
         if self.current_layer == 0:
@@ -372,7 +417,7 @@ def main():
                     current_block.x, current_block.y = pygame.mouse.get_pos()
             elif event.type == pygame.MOUSEBUTTONUP:
                 if event.button == 1 and current_block:
-                    if tower.add_block(current_block):
+                    if tower.add_block_from_sim(current_block):
                         current_block = None
                     else:
                         print("CANNOT place block here, there is already a block there.")
@@ -385,9 +430,12 @@ def main():
                 elif event.key == pygame.K_a:  # 按 'A' 键自动放置
                     auto_block = tower.auto_place_block()
                     if auto_block:
-                        tower.add_block(auto_block)
+                        if tower.add_block_from_sim(auto_block):
+                            print(f"成功放置积木在 ({auto_block.x}, {auto_block.y}) 角度为 {auto_block.angle}")
+                        else:
+                            print("无法放置积木")
                     else:
-                        print("Cannot find a suitable position for auto-placement.")
+                        print("无法找到合适的放置位置")
                 elif event.key == pygame.K_n:  # 按 'N' 键自动放置新的一层
                     tower.auto_place_layer()
                 elif event.key == pygame.K_s:  # 按 'S' 键扫描当前层