# 双臂魔方机器人（闭环版本）

**Repository Path**: zhangzq71/dual-arm-rubiks-cube-robot-v2

## Basic Information

- **Project Name**: 双臂魔方机器人（闭环版本）
- **Description**: 使用4个闭环步进电机控制的双臂双指解魔方机器人，支持力矩控制，可控制夹紧压力，防止魔方滑落。
- **Primary Language**: Python
- **License**: MIT
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 11
- **Created**: 2025-06-02
- **Last Updated**: 2025-08-19

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 说明 😈
使用 4 个闭环步进电机控制的双臂双指解魔方机器人，支持力矩控制，可控制夹紧压力，防止魔方滑落。

机械部分分齿轮传动版本和同步带传动两个版本。

齿轮传动版本大部分部件使用 FDM 3D 打印方式制作，优缺点如下：

✨ **优点：**
*   成本低，方便加工

❗ **缺点：**
1.  旋转臂晃动较大，极限速度低
2.  强度有限，程序 bug 导致的异常状态下，结构件容易损坏

同步带传动版本大部分部件使用标准件、以及 CNC 加工部件，优缺点如下：

✨ **优点：**
1.  旋转臂晃动极小，还原速度快，可靠性高
2.  强度高，异常碰撞不会损坏

❗ **缺点：**
*   加工成本较高

**后续软件开发，只维护同步带传动版本**，两者的控制方式几乎一致，但是部分参数不同，如有需要可自行移植调试。

软件部分esp32版本和电脑版本
1.  电脑版本使用python编写，搭配USB摄像头和USB转485模块使用，主要用于算法验证，也可以直接使用。
2.  esp32版本使用c编写，基于ESP32S3单片机开发，可独立使用，不依赖其他设备，提供硬件原理图、PCB和软件源码。

---

# 同步带传动版本 🚀
![装配图](img/belt.png)

## 🛠️ 部件加工说明
### 🔩 CNC 件加工说明
> 除了齿条，设计文件均适配铨XXX的智能制造工艺，可低成本打样，10个CNC件100多元。
> 推荐使用 6061 铝合金材料。
> 运动部件的重量会影响性能，不建议用密度太高的材料（黄铜、钢之类的）。
> 魔方机器人--手指-加工4个.step、魔方机器人--手指--4拼板--加工1个.step任选一个加工即可。（拼版的需要自己掰开）

1.  **`魔方机器人--42电机固定座-右侧.step`**
    *   加工 1 个。
    *   如有条件，可精确控制环形凸台的外径，与 6700ZZ 轴承的内径紧配。
    *   如果没有条件，也可以留一点空隙，用 638 圆柱部件固持胶粘接。强度是满足要求的，实测 20kg 左右的拉力不松动。
2.  **`魔方机器人--42电机固定座-左侧.step`**
    *   加工 1 个，同上。这两个部件的外形是镜像关系。
3.  **`魔方机器人--手指-加工4个.step`**
    *   加工 4 个。
    *   与旋转臂接触的几个面尽量光滑平整。如果不平整，可通过后期砂纸打磨解决。
    *   部件上的 2 个孔，需要加工 M2 螺纹。
4.  **`魔方机器人--齿条-加工4个.step`**
    *   加工 4 个。
    *   可用 2mm 平底铣刀和 40° 0.2mm 平底尖刀加工。
    *   也可以考虑用成品 0.5 模齿条切割打孔制作。
5.  **`魔方机器人--旋转臂顶盖--加工2个.step`**
    *   加工 2 个。
    *   部件上的 4 个孔，需要加工 M2 螺纹。
6.  **`魔方机器人--旋转臂底部-含侧面扳手孔.step`** 或 **`魔方机器人--旋转臂底部-不含侧面扳手孔--加工2个.step`**
    *   选择其中某一文件，加工 2 个。
    *   侧面扳手孔用于拧凸台齿轮的固定螺丝，对精度要求很低，可以自己用 2mm 麻花钻加工。如果用 CNC，需要多装夹一次。

### 🖨️ 3D 打印件说明
**文件：** `魔方机器人--底座--加厚版本.stl`，加工 1 个
**文件：** `魔方机器人--ESP32摄像头支架.stl`，加工 1 个

| 参数项           | 设置值                     |
|------------------|--------------------------|
| 打印技术         | FDM 3D 打印              |
| 推荐耗材         | PETG/ABS                 |
| 喷嘴直径         | 0.4mm                    |
| 层高             | 0.2mm                    |
| 结构厚度         | 壁厚/顶/底层均 2mm       |
| 填充率           | 40-90% (推荐 80%)        |
| 支撑             | 不需要                   |

## 📦 物料清单 (BOM)
### 🔧 核心部件
| 编号 | 名称                                     | 规格参数                                  | 数量 | 备注                                                                 |
|------|------------------------------------------|-----------------------------------------|------|----------------------------------------------------------------------|
| 1    | 6700ZZ 滚珠轴承                          | 10x15x4mm                               | 4    |                                                                      |
| 2    | 步进电机                                 | 48 长度，轴长 24mm±1mm                  | 2    | 支持侧/底部出线，电机凸台高度 2-3mm，结构设计也**兼容 42 无刷电机** |
| 3    | 步进电机                                 | 48 长度，轴长 17mm±1mm                  | 2    | 可用买标准轴长的，自己切短一点                                       |
| 4    | 径向磁铁                                 | 6x2.5mm                                 | 4    | 闭环控制板用                                                       |
| 5    | 凸台齿轮                                 | 0.5 模 20 齿                            | 2    |                                                                      |
| 6    | 同步带轮                                 | 2GT-40 齿-AF-齿宽 7-内孔 15             | 2    |                                                                      |
| 7    | 同步带轮                                 | 2GT-20 齿-AF-齿宽 7-内孔 5              | 2    |                                                                      |
| 8    | 刚性法兰盘联轴器                         | 内径 5                                  | 1    | 用作摄像头支架                                                       |
| 9    | 5mm 直径金属棒                           | 长度 300mm                              | 1    | 用作摄像头支架，尺寸要求不高，买光轴，铁棒，钢筋之类的都行           |
| 10   | 2GT 同步带                               | 6mm 宽度，148mm 长度                    | 2    |                                                                      |
| 11   | 闭环 42 电机控制板                       |                                         | 4    | 参考另一个开源项目：https://gitee.com/hemn1990/closed-loop-stepper-motor  后续考虑做闭环的 42 无刷电机（伺服电机） |

> **注：** 部件的具体尺寸，可参考 CAD 设计文件，可用开源的 FreeCAD（版本号 1.0 或者更高）打开。

🔗 **部分器件的参考购买链接（仅供参考）：**
*   同步带轮：[https://item.taobao.com/item.htm?id=632391487295](https://item.taobao.com/item.htm?id=632391487295)
*   齿轮：[https://item.taobao.com/item.htm?id=806227801033](https://item.taobao.com/item.htm?id=806227801033)
*   步进电机：[https://detail.tmall.com/item.htm?id=623890275768](https://detail.tmall.com/item.htm?id=623890275768)
*   滚珠轴承：[https://item.taobao.com/item.htm?id=771754646342](https://item.taobao.com/item.htm?id=771754646342)
*   法兰盘联轴器：[https://item.taobao.com/item.htm?id=554246764479](https://item.taobao.com/item.htm?id=554246764479)
*   同步带：[https://item.taobao.com/item.htm?id=731066241303](https://item.taobao.com/item.htm?id=731066241303)

### 🔩 紧固件
| 类型                  | 规格       | 数量 | 备注                             |
|----------------------|------------|------|----------------------------------|
| M3 杯头内六角螺丝     | M3*6       | 32   |                                  |
| M2 杯头内六角螺丝     | M2*4       | 16   |                                  |
| M3 杯头内六角螺丝     | M3*45       | 16   | 闭环控制板                       |
| 热熔螺母             | M3x4x5     | 16   | 外径 5mm，光杆直径 4.3mm         |
| M2十字平头自攻螺丝    | M2*8       | 2    | 用于固定摄像头                    |
| M3六角螺母           | M3         | 1    | 用于固定摄像头                    |
| M3 杯头内六角螺丝     | M3*25      | 2    | 用于固定摄像头                    |
### 📎 其他
*   M4x25 尼龙脚杯 × 4
*   5mm 塑料柱 × 16（闭环控制板支撑用，根据径向磁铁的安装高度调整）
*   638 厌氧胶水/圆柱形部件固持胶（不可以用 502 之类的通用胶水代替，不耐震动容易脱落）
*   润滑脂
*   2mm厚度硅胶垫（带双面胶）

## 🧩 装配步骤
1.  **安装轴承：** 将 4 个 6700ZZ 滚珠轴承分别安装在 42 电机固定座的环形凸台上，每侧 2 个。根据情况选择：
    *   用台钳压入（紧配）
    *   或用 638 胶水粘接（间隙配合）。
    *   ⚠️ **注意：** 千万不要把胶水弄到轴承内部！638 的粘度较高，不容易出现这个问题。如果用流动性更好的胶水，需要特别注意。
2.  **安装同步带轮：** 将 2 个 40 齿同步带轮，分别安装在旋转臂底部组件的环形凸台上，方式同上（压入或胶粘）。
3.  **固化胶水：** 等待几分钟，擦掉多余胶水，然后等待剩余的 638 胶水固化。
4.  **安装热熔螺母：** 在底座的 16 个盲孔上，加热压入热熔螺母。（通孔是用来安装摄像头支架的，不需要装螺母）
5.  **固定底座部件：** 将法兰盘联轴器、42 电机固定座用 M3 螺丝固定在底座上。
6.  **安装步进电机：** 安装 4 个步进电机。腰孔的螺丝暂时不需要锁紧。
7.  **安装金属棒：** 安装 5mm 直径金属棒（摄像头支架）。
8.  **安装同步带系统：**
    *   安装大小两种同步带轮和同步带。
    *   ⚠️ **注意：** 大同步带轮（40齿）的顶丝**不要太紧**！这个顶丝是顶到轴承侧壁的，轴承侧壁不能承受过大的压力。
9.  **张紧同步带：**
    *   在两个电机之间塞入厚度合适的物品（例如一字螺丝刀）以张紧同步带。
    *   锁紧腰孔的螺丝以固定电机位置。
10. **组装手指：**
    *   将齿条和手指组装在一起。
    *   使用 M2 螺丝锁紧。
    *   需要控制垂直度，可以夹在台钳的 90° 面上安装。
11. **安装齿轮：**
    *   安装 0.5 模 20 齿齿轮。
    *   通过旋转臂的侧孔，插入内六角螺丝刀锁紧固定螺丝。
    *   ⚠️ **注意：** 在不滑丝的前提下，锁紧力度尽可能大，否则可能会脱落。
    *   💡 **建议：** 可以考虑用螺纹胶（中低强度）。千万别用圆柱部件固持胶（如638），强度极高，粘上就很难拆卸。
12. **润滑与组装：**
    *   在滑动部位（如齿条、轴承）涂抹适当润滑脂。⚠️ **注意：** 同步带不需要润滑！
    *   安装旋转臂剩余部件（顶盖等）。
13. **测试与调整：**
    *   测试手指组件的滑动是否顺畅。
    *   旋转旋转臂组件，手指应当发生滑动。
    *   如果不顺滑，可适当打磨齿条和手指组件。可通过观察磨损位置，确定要打磨的点位。
14. **粘贴硅胶垫：**
    *   将硅胶垫裁剪为12x11mm的尺寸，分别粘贴在4个手指的尖端

---

# 齿轮传动版本 ⚙️
![装配图](img/gear.png)

## 🖨️ 打印参数说明
### 🧱 基座专用设置 (`魔方机器人基座.stl`)
| 参数项           | 设置值                     |
|------------------|--------------------------|
| 打印技术         | FDM 3D 打印              |
| 推荐耗材         | PETG/ABS                 |
| 喷嘴直径         | 0.4mm                    |
| 层高             | 0.2mm                    |
| 结构厚度         | 壁厚/顶/底层均 2mm       |
| 填充率           | 40-90% (推荐 80%)        |
| **特殊要求**     | 标记面（4, 3, 1, 2）朝 Z 轴正方向打印 |

### 🔧 其他部件通用设置
| 参数项           | 设置值                     |
|------------------|--------------------------|
| 打印技术         | FDM 3D 打印              |
| 推荐耗材         | PETG/ABS                 |
| 喷嘴直径         | 0.4mm                    |
| 层高             | 0.1mm                    |
| 结构厚度         | 壁厚/顶/底层均 2mm       |
| 填充率           | 40-90% (推荐 80%)        |

> ⚠️ **注意：** PLA 材料因脆性和时效强度衰减问题，不推荐用于任何结构件打印。

⚠️ **关键注意事项**
1.  推荐提前打印 `孔径校准圆柱.stl`，调整水平扩展参数使内径 ≈ 5mm。
2.  使用大象脚补偿功能确保齿轮工作面平整。
3.  **尺寸标注文件选择指南：**
    *   外圆偏小 → 选择 `旋转臂主体-17.2mm.stl`
    *   外圆偏大 → 选择 `旋转臂主体-17.0mm.stl`

## 📦 物料清单 (BOM)
### 🔧 核心部件
| 编号 | 名称                      | 规格参数                     | 数量 | 备注                     |
|------|--------------------------|----------------------------|------|--------------------------|
| 1    | 黄铜六角联轴器           | 对边 12mm×长 18mm×孔 5mm   | 2    |                          |
| 2    | 热熔螺母                 | M3x4x5                     | 6    | 外径 5mm，光杆直径 4.3mm |
| 3    | 6903 滚珠轴承            | 17×30×7mm                  | 2    |                          |
| 4    | 步进电机                 | 48 长度                    | 4    | 支持侧/底部出线          |
| 5    | 径向磁铁                 | 6x2.5mm                    | 4    | 闭环控制板专用           |

### 🔩 紧固件
| 类型                  | 规格              | 数量 | 备注                 |
|----------------------|-------------------|------|----------------------|
| M2x8 十字平头自攻螺丝 | 头径 ≤ 4.2mm      | 24   |                      |
| M3 机米螺丝          | 6mm               | 4    |                      |
| M3 杯头内六角螺丝     | 8mm               | 16   |                      |
| M3 杯头内六角螺丝     | 45mm              | 16   | 闭环控制板专用       |

### 📎 选配件
*   M4x25 尼龙脚杯 × 4
*   5mm 塑料柱 × 16（闭环控制板支撑用，根据径向磁铁的安装高度调整）

## 🧰 调试工具包
1.  **必备工具** 🛠️
    *   1.5mm 麻花钻（扩孔用）
    *   螺丝刀套装
    *   游标卡尺
    *   3D 打印机

2.  **电子调试工具** 🔌
    *   万用表
    *   示波器
    *   USB 转 485 转换器
    *   可调直流电源

3.  **辅助工具** 🧪
    *   焊台 + 热风枪
    *   镊子
    *   打磨工具组

💻 **开发环境**
支持跨平台开发（Windows/macOS/Linux）


# 硬件调试说明（hardware_esp32）
如果在嘉立创制作PCB，选择JLC04161H-7628叠层，板厚1.6mm
## 演示视频与调试指南
[视频教程](https://www.bilibili.com/video/BV1aRhgz5EEY/)

## 硬件实物图
| 控制板顶视图 | 控制板底视图 |
|--------------|--------------|
| ![top](img/top.jpg) | ![bot](img/bot.jpg) |

| 整机视图1 | 整机视图2 |
|-----------|-----------|
| ![robot1](img/robot1.jpg) | ![robot2](img/robot2.jpg) |

## 文件说明
| 文件名称 | 描述 |
|----------|------|
| `魔方机器人ESP32主控板_2025-07-08.zip` | PCB制版文件（Gerber格式） |
| `2025-07-08.epro` | 嘉立创EDA工程文件（含原理图+PCB设计） |
| `物料清单.xlsx` | 主控板BOM表 |

## 核心器件清单
| 数量 | 型号 | 位号 | 功能描述 |
|------|------|------|----------|
| 1 | ESP32-S3R8 | U1 | 主控芯片（双核240MHz，8MB PSRAM） |
| 1 | ME6206A28XG | U2 | 2.8V LDO稳压器 |
| 1 | ME6206A15XG | U3 | 1.5V LDO稳压器 |
| 1 | W25Q64JVSSIQ | U4 | 64Mbit SPI Flash存储器 |
| 1 | SY7200AABC | U5 | LED驱动器 |
| 1 | JW5026 | U9 | 24V转3.3V DC-DC转换器 |
| 1 | SP3485EEN(UMW) | U11 | RS485通信芯片 |
| 1 | OV3660 | - | 300万像素摄像头（24pin FPC接口） |

📌 **摄像头采购链接**：  
[OV3660摄像头（21mm排线）](https://item.taobao.com/item.htm?id=627789434982)

## 推荐调试步骤
1. **焊接顺序**：
   - 先焊接TOP面贴片器件
   - 再焊接BOT面贴片器件
   - 最后焊接TOP面直插器件
   - 安装跳线帽和摄像头模块

2. **电源检查**：
   - 测量各电源对地电阻（3.3V/1.5V/2.8V）
   - 上电测量电压
     - 3.3V（主控供电）
     - 1.5V（摄像头供电）
     - 2.8V（摄像头供电）

3. **程序下载**：
   - 通过USB接口烧录固件
   - 参考软件调试说明进行功能验证

# 软件调试说明（software_esp32）

## 各模块功能说明

1. **颜色检测模块 (color_detect.c)**  
   **功能**：从摄像头图像中识别魔方54个色块的颜色  
   **运行机理**：
   - 基于透视变换划分18个色块区域
   - 使用二维直方图提取每个区域的主色调（HSV空间）
   - 实现RGB565↔HSV转换算法
   - 通过聚类算法将54个色块分类为6个面
   - 提供色差计算和颜色相似度判断函数

2. **主控制模块 (main.c)**  
   **功能**：系统总控，协调各模块工作  
   **运行机理**：
   - 初始化WiFi热点和HTTP服务器（80/81端口）
   - 管理摄像头采集任务和图像处理流程
   - 实现状态机控制（10种状态处理魔方还原流程）
   - 提供Web界面接口：
     - 实时视频流传输（MJPEG）
     - 参数配置（亮度、坐标点等）
     - 运动控制和状态监控
   - 处理SPIFFS文件系统操作（配置文件读写）
   - 协调颜色识别与运动控制模块

3. **运动控制模块 (motion.c)**  
   **功能**：精确控制4个步进电机完成魔方操作  
   **运行机理**：
   - RS485通信协议实现（1Mbps）
   - 梯形速度曲线运动控制
   - 多轴联动算法（手臂旋转与手指移动协同）
   - 自动回零和位置校正
   - 动作序列解析执行（如"R1 L*F"等指令）
   - 堵转检测和超时保护
   - 电流自适应控制（夹紧时提高电流）

4. **求解器核心模块 (solve.c)**  
   **功能**：实现魔方求解的两阶段算法 (Kociemba算法)  
   **运行机理**：
   - **状态表示转换**：
     - 将54面颜色表示转换为棱块/角块坐标表示 (`UtilToCubieCube`)
     - 验证魔方状态合法性 (`CubieCubeVerify`)
   - **两阶段搜索**：
     - **Phase1**：快速降群到<U,D,L,R,F2,B2>子群
       - 计算翻转/扭转/UDSlice坐标
       - 使用剪枝表加速搜索 (`CoordCubeSetWithPrun`)
     - **Phase2**：在子群内完成求解
       - 处理角块/棱块排列对称性 (`CubieCubeGetPermSymInv`)
       - 多方向并行搜索 (6种URF对称方向)
   - **解法优化**：
     - 动作序列压缩 (`SolutionAppendSolMove`)
     - 支持解法格式控制 (逆序输出/添加分隔符等)

5. **动作优化模块 (optimizer.c)**  
   **功能**：将魔方解法转换为机械臂动作序列  
   **运行机理**：
   - **状态机建模**：
     - 定义96种机械臂状态 (24种魔方朝向 × 4种手臂状态)
     - 使用96×96状态转移表 (`flip_table`)
   - **动态路径规划**：
     - DP表存储最优路径 (`DPState`结构)
     - 状态转移代价基于实测时间 (`TimeCost`枚举)
   - **动作序列生成**：
     - 翻转序列池包含83种预计算动作 (`flip_sequences`)
     - 旋转操作处理手臂协同运动 (`calculate_twist_time_and_state`)
     - 最终位置自动校准 (`adjust_final_position`)

6. **打乱生成模块 (scramble.c)**  
   **功能**：生成随机打乱状态及解法验证  
   **运行机理**：
   - **状态表示**：
     - 棱块/角块的三维表示 (`cube_struct`)
     - 54面与棱块/角块的映射表 (`edge_to_face`, `corner_to_face`)
   - **随机打乱**：
     - 使用硬件RNG生成50~69步随机操作 (`esp_random`)
     - 动作序列执行 (`cube_route_str`)
   - **解法验证**：
     - 逆解法生成 (`reverse_solution`)
     - 状态一致性检查 (`verify`)

7. **WEB前端（index.html）**  
   **功能**：魔方机器人调试网页提供4大核心功能  
   **运行机理**：
   1. **设备控制**：通过按钮实现电机使能/禁用、机械臂回零、魔方夹紧/松开等操作，支持自定义动作序列测试（如"R+T"表示右电机正转）
   2. **视觉标定**：
      - 实时显示摄像头画面
      - 提供6个可拖拽标记点用于标定魔方识别区域
      - 支持坐标保存（点击"保存坐标"按钮）
   3. **颜色识别**：
      - 动态展示54个魔方块的颜色识别结果
      - 以3D展开图形式呈现（9×12网格布局）
      - 每个色块显示编号和识别颜色（如"0:W"表示0号白色块）
   4. **状态监控**：
      - 实时显示系统连接状态
      - 反馈电机角度数据
      - 补光灯亮度调节（0-100滑动条）
   - 所有功能通过WebSocket与ESP32设备实时通信
   - 界面采用响应式设计，适配PC和移动设备操作

8. **配置文件的在线管理（config.html）**  
   **功能**：设备配置文件的在线管理  
   **运行机理**：
   1. **文件管理**：
      - 支持4种配置文件操作：`motion.cfg`（动作参数）、`main.cfg`（主配置）、`testcase.txt`（测试用例）、`index.html`（主界面）
   2. **核心操作**：
      - **打开**：从设备加载选定配置文件
      - **保存**：将编辑内容写入设备存储器
      - **下载**：将配置文件下载到本地电脑
   - 所有操作通过HTTP接口（`/cfg/get`, `/cfg/save`, `/cfg/download`）与ESP32设备交互
   - 实现配置文件的远程管理，无需物理访问设备即可修改关键参数
   - 界面简洁高效，专为开发调试设计

## 常见故障排查

### 故障现象：无法正常回零点
**故障日志**：  
```
E (1725) motion: 手臂电机堵转故障  
E (1730) main: cmd_zero failed.  
```
**排查方式**：  
1. 调整motion.cfg中的ARM*_ZERO  
2. 回零点时，不要放置魔方  

### 故障现象：电机不动
**故障日志**：  
```
E (1333) motion: 读取串口失败  
E (1333) motion: 接收响应失败  
E (1333) motion: 4号电机通信不稳定  
```
**排查方式**：  
1. 检查主控板和电机控制板之间的连线是否断开  
2. 检查电机控制板的固件是否正常  

### 故障现象：还原过程中魔方飞出
**排查方式**：  
1. 使用手机的慢动作功能录制视频，找出性能瓶颈，调整motion.cfg中的速度参数  
2. 尝试通过修改SPEED_FACTOR参数，按照比例降低整体还原速度  

---

### 故障现象：采集到的魔方颜色信息不正确
**故障日志**：  
```
I (228350) main: 魔方图像识别结果：UUUUUUUUURRRRRRRRRFFFFFFFFFDDDDDDBDDLLLLLLDLLBBBBBBBBL  
W (228354) solve: SearchSolution Error 5: Twist error: One corner has to be twisted  
I (228361) main: 魔方解法:  (-5步)  
```

**排查方式**：  
1. 检查实时视频中的魔方角点标记是否正确  
2. 检查图像是否清晰  
3. 检查魔方是否可以手工还原，有没有拧角、棱块组装错误等问题  

### 故障现象：放上魔方后不开始还原
**故障日志**：  
```
I (22597) main: 图像已经稳定0帧，目前变化像素比例2.23958%，等待超时，重新检测魔方放置  
I (22623) main: 差异色块数量10, 检测到魔方放置, 等待图像稳定  
```
**排查方式**：  
1. 修改main.cfg中的图像稳定条件：  
   ```ini
   # 需要的稳定帧数
   STABILIZATION_FRAMES = 4
   # 图像静止的像素百分比
   STABILIZATION_PERCENT = 0.05
   # 像素变化阈值
   THRESH_DIFF = 10
   ```
2. 检查摄像头拍摄范围内是否存在移动物体（如人手）  
3. 检查main.cfg配置：`AUTO_CUBE_DETECT = 2`时才会自动还原  
   ```ini
   # 是否开启自动检测魔方放置的功能
   # 0: 不检测 1: 检测但不还原 2: 检测并自动还原
   AUTO_CUBE_DETECT = 0
   ```

### 故障现象：视频流无显示
**排查方式**：  
1. 点击开启视频流选项  
2. 注意两个设备同时连接时，只有一个设备会显示视频流  


### 故障现象：图像过曝，黄色区域变成白色
**排查方式**：  
降低main.cfg中的自动曝光级别：  
    ```ini
    # 自动曝光级别 (-5 ~ +5)
    CAMERA_AE_LEVEL = -3
    ```


# 软件调试说明（software）

## 各程序功能与运行机理

### 1. cube_color_detect.py - 自适应魔方颜色识别
![魔方块分割与标记结果](img/blocks_with_colors_results_annotated.jpg)
![基于hsv色彩空间的自适应颜色分裂](img/lab_clustering.png)
**功能**：  
1. 从三张不同角度的魔方照片中识别54个色块的颜色，输出魔方状态字符串
2. 自适应色彩识别，除了需要提前标定魔方所在的区域，不需要配置其他参数

**运行流程**：
1. 加载三张魔方照片和六个定位点
2. 对每张照片进行透视变换，分割出18个色块（三张共54个）
3. 对每个30x30像素的色块：
   - 使用改进的K-means聚类算法提取主色调
   - 动态合并相似颜色
   - 标记非主色区域为透明粉色
4. 基于色彩空间，对54个主色调进行固定大小聚类（6类）
5. 将聚类结果映射到魔方展开图
6. 输出魔方状态字符串（54个字符）

**调试要点**：
```python
# 启用绘图调试（会显示处理过程）
need_plot = True
```

### 2. cube_motion.py - 机械臂运动控制
**功能**：  
通过串口控制4个步进电机完成魔方的夹持、旋转和翻转动作

**核心控制逻辑**：
```python
# 运动控制指令示例
mc.motions(['R1', 'L*F', 'R0'])  # 执行一组动作

# 动作类型说明：
# L0/R0: 夹爪夹紧
# L1/R1: 夹爪松开
# L2/R2: 夹爪张开最大角度
# L+T/L-T/L*T(L可替换为R): 90°顺时针/逆时针旋转/180°旋转
# L+F/L-F/L*F(L可替换为R): 90°/180°翻转
# L+N/R+N: 旋转臂快速空转90°

# 动作举例
# L1 R-F L0 R2 R+N R0 L+T R1 L+F R0 R-T R2 R+N R0 L*T L1 R-F L0 R2 R+N R0 ......
```

**通信协议**：
1. 使用自定义帧结构（FF FF开头）
2. CRC8校验（多项式0x07）
3. 支持单指令多电机控制

**调试要点**：
```python
# 关键参数调整：
SPEED_FACTOR = 1.0  # 全局速度因子（建议从0.2开始调试）
ARM4_ZERO = 15095   # 左侧旋转臂零点
ARM2_ZERO = 803     # 右侧旋转臂零点
```

### 3. cube_optimizer.py - 动作序列优化
**功能**：  
将魔方解法转换为机械臂最优动作序列，减少总执行时间

**优化算法**：
1. 状态空间：(当前夹持面, 左臂垂直状态, 右臂垂直状态)
2. 使用BFS搜索2步内的翻面路径
3. 动态规划选择耗时最短的路径

**调试要点**：
```python
# 搜索深度，数值越大，结果越精确，但是搜索算法耗时越长
if new_depth < 2:
    queue.append((s1, l1, r1, new_flip_time, new_flip_sequence, new_depth))
```

### 4. cube_robot_test.py - 主控程序
**功能**：  
集成颜色识别、运动控制和优化模块，实现完整的魔方还原流程

**工作流程**：
1. 初始化摄像头和串口
2. 用户交互界面：
   - 实时显示摄像头画面
   - 可拖动6个点标定魔方位置
3. 按空格键启动还原：
   ```mermaid
   graph TD
   A[夹紧魔方] --> B[拍摄三张照片]
   B --> C[颜色识别]
   C --> D[Kociemba求解]
   D --> E[动作序列优化]
   E --> F[执行机械臂动作]
   F --> G[松开魔方]
   ```

### 5. verify_arm_finger_linkage.py - 运动规划验证
**功能**：  
模拟手臂和手指的联动运动，优化空转90°的运动参数

**输出参数**：
```python
V_NO_LOAD_20_70_DEG = 846  # 手臂20°-70°区间的空转速度
FINGER_NO_LOAD_START_ARM = 63.98  # 手臂开始空转时手指的位置
```

## 调试流程指南

### 准备工作
1. **硬件连接**：
   - 确保4个电机正确接线
   - 摄像头对准魔方区域
   - 所有设备供电正常

2. **环境配置**：
   ```bash
   pip install opencv-python numpy matplotlib pyserial kociemba
   ```

### 分模块调试步骤

**颜色识别模块**：
1. 准备三张魔方照片（不同角度）
2. 运行`cube_color_detect.py`
3. 检查输出：
   - 透视变换后的色块图
   - HSV空间聚类结果
   - 魔方状态字符串

**运动控制模块**：
1. 连接电机控制器
2. 运行`cube_motion.py`
3. 使用交互菜单：
   ```python
   [1]: 使能全部电机
   [2]: 禁用全部电机
   [3]: 查询电机角度
   [4]: 回零点(需要先回零点才能执行其他的！)
   [5]: 松开魔方
   [6]: 夹紧魔方
   [7]: 预留
   [8]: 测试旋转臂动作
   [9]: 打乱魔方再还原
   [q]: 退出程序
   ```

**主控程序调试**：
1. 运行`cube_robot_test.py`
2. 在摄像头画面中拖动6个点标定魔方
3. 按空格键开始自动还原

## 常见问题解决

1. **颜色识别不准确**：
   - 优化光照条件
   - 反光面积不可以超过单个色块的1/3
   - 魔方必须包含一个白色面

2. **机械臂动作异常**：
   ```python
   # 调整零点参数（根据实际位置）
   # 执行cube_motion.py
   # 选择[2]: 禁用全部电机
   # 人工移动旋转臂，使手指落在魔方的正中央
   # 选择[3]: 查询电机角度
   # 修改代码中的零点位置
   ARM4_ZERO = 15095   # 左侧旋转臂零点
   ARM2_ZERO = 803     # 右侧旋转臂零点
   ```

3. **魔方定位偏差**：
   - 确保6个标定点包围魔方

4. **串口通信错误**：
   - 检查端口号，当打开默认串口失败时，会自动列出可用串口
   ```
   可用串口设备:
   /dev/cu.wlan-debug - n/a
   /dev/cu.Bluetooth-Incoming-Port - n/a
   /dev/cu.usbserial-120 - USB Serial
   、、、
   - 检查电机控制板的状态

## 性能优化建议
1. **运动优化**：
   - 减小`SPEED_FACTOR`提高稳定性
   - 增大`SPEED_FACTOR`提高速度
   - 可单独修改翻转、旋转、空转、手指伸缩动作的速度


2. **典型性能指标**
   - 目前版本，从开始拍照，到魔方还原，耗时在6-7秒之间
   - 程序还没有完全开发完成，预计可优化到5秒之内

# 许可证
采用 **MIT开源协议**，允许商业用途。