# 调度车调度逻辑

> 本文档阐述从视觉扫描完成到送料调度车取料、送至立锯加工完成的完整调度加工逻辑，涵盖升降调度车送料、取料回退、立锯分配、等待位调度、龙门取料入锯及立锯切割加工全链路。

---

## 1. 整体流程概览

```mermaid
graph TB
    A[升降调度车送料到视觉拍照位] --> B[视觉3D扫描 + 算法处理]
    B --> C[视觉顶针旋转完成 M46=true]
    C --> D[sawDispatchTrigger通道触发]
    D --> E{立锯空闲判断<br/>B优先}
    
    E -->|有空闲| F[取料 → 回退]
    E -->|都繁忙| G[取料 → 回退 → 等待位]
    
    F --> H[送料到立锯]
    G --> I[轮询监控立锯空闲 → 分配]
    
    H --> J[龙门取料入锯]
    I --> K[送料到立锯 → 龙门取料]
    
    J --> L[立锯切割加工]
    K --> L
```

---

## 2. 时序图

```mermaid
sequenceDiagram
    participant 升降车 as 升降调度车
    participant 视觉 as 视觉算法系统
    participant 上位机 as go-auto上位机
    participant PLC as 主控PLC
    participant 送料车 as 送料调度车
    participant 龙门 as 龙门(A/B)
    participant 立锯 as 立锯(A/B)

    升降车->>PLC: D2200=2630 送视觉拍照位
    PLC->>上位机: X72=true(升降车有料)
    上位机->>视觉: start_vision_algorithm
    视觉-->>上位机: capture_ready
    上位机->>PLC: M103(启动扫描轨道)
    上位机->>视觉: order_info(扫描结束)
    视觉-->>上位机: algorithm_coordinates(切割坐标)
    Note over PLC: M46=true(视觉顶针旋转完成)

    alt 立锯有空闲
        上位机->>上位机: sawDispatchTrigger触发
        上位机->>上位机: B优先分配立锯,更新DB
        上位机->>PLC: M57(启动取料)
        PLC-->>上位机: M58=true(取料完成)
        上位机->>PLC: M117(顶针回退)+M121(扫描架回退)
        PLC-->>上位机: M118+M122=true(回退完成)
        上位机->>上位机: 安全位检查(龙门坐标)
        上位机->>PLC: D2044=目标坐标+M59(启动送料)
        PLC-->>上位机: D2042回读验证
    else 立锯都繁忙
        上位机->>PLC: M57(启动取料)
        PLC-->>上位机: M58=true(取料完成)
        上位机->>PLC: M117+M121(回退)
        PLC-->>上位机: M118+M122=true(回退完成)
        上位机->>PLC: D2194=2300+M107(等待位)
        PLC-->>上位机: M52=true+D2042=2300(到达等待位)
        上位机->>上位机: 轮询监控A/B立锯空闲
        上位机->>上位机: 立锯空闲后分配,通道通知龙门
    end

    上位机->>龙门: gantryDispatchTrigger通知
    龙门->>PLC: M67/M85(龙门取料)
    PLC-->>龙门: M68/M86=true(取料完成)
    龙门->>PLC: M73/M91(龙门入锯)
    PLC->>立锯: M74/M92上升沿(触发加工)
    立锯->>立锯: 写入坐标→切割→避让→回原
```

---

## 3. 升降调度车送料（前置流程）

升降调度车负责将原木从储料区送至视觉拍照位，是送料调度车取料的前置步骤。

### 3.1 控制逻辑

| 逻辑 | 触发条件 | 动作 | 说明 |
|------|---------|------|------|
| 顶料 | D2130在原点(0±0.01) + X56=true + M19=true | 写入M25上升顶料 | 升降车回到原位且有料时触发 |
| 送视觉拍照位 | M41=true + D2042≥2300 + X72=true + M26=true | D2200=2630 + M808启动 | 视觉顶针空闲时送到拍照位 |
| 送等待位 | M41=false + X72=true | D2200=1100 + M808启动 | 视觉顶针繁忙时送到等待位 |
| 升降回原 | D62>60 + D2130在拍照位(2630±5) | 写入M113启动回原 | M114上升沿重置允许下次回原 |

### 3.2 关键坐标

| 寄存器 | 含义 | 值 |
|--------|------|-----|
| D2200 | 升降车目标位 | 2630(拍照位) / 1100(等待位) |
| D2130 | 升降车实时坐标 | 0=原点 |
| X72 | 升降车有料检测 | bool |
| M808 | 启动送料 | bool |

---

## 4. 视觉扫描完成触发调度

视觉算法处理完成后，通过 `sawDispatchTrigger` 通道事件驱动调度逻辑启动。

### 4.1 触发机制

```
视觉算法完成 → 上位机异步处理器 → sawDispatchTrigger通道写入
    → monitorSawDispatchTrigger()消费事件
    → pendingSawTaskCount原子计数器+1
```

### 4.2 立锯分配策略（B优先）

```
pendingSawTaskCount++
    ↓
检查B立锯空闲？ ──是──→ 分配B立锯, D2044=8522
    │否
检查A立锯空闲？ ──是──→ 分配A立锯, D2044=4000
    │否
都繁忙 → 启动取料+等待位流程
```

**分配动作**（`passCoordinatesToSaws`）：
1. **立锯置忙**：通过 `SawProcessor.currentWoodID.Store(WoodID)` 立即置忙，防止窗口期重复分配
2. **DB更新**：`saw_type`=A/B, `process_status`=加工中(1)
3. **LED推送**：调用 `updateLEDSawStatus()` 更新LED面板显示

---

## 5. 送料调度车取料流程

### 5.1 取料触发条件（逻辑2）

```
M46=true(视觉顶针旋转完成)
  && M51=true(送料调度车空闲)
  && D2042=2300±0.1mm(调度车在取料位)
  && visionProcessing=false(视觉处理安全标志)
  → 写入M57=true(启动取料)
```

### 5.2 取料完成检测（逻辑3）

```
M58上升沿(false→true) = 取料完成
  → 并行写入:
    M117=true(视觉顶针回退)
    M121=true(相机扫描架回退)
```

### 5.3 回退完成监控（逻辑4）

```
M118上升沿(顶针回退完成)
  && M122上升沿(扫描架回退完成)
  → 根据sawAvailable决定后续动作:
    true  → executeDispatchToSaw(送料到立锯)
    false → executeDispatchToWaitPosition(送到等待位)
```

> 超时保护：M58检测超时3600秒，M118/M122检测超时30秒

---

## 6. 送料到立锯

### 6.1 安全位检查

送料到立锯前，必须先通过动态安全位检查（`dispatcher_sorting_safety.go`）：

**送料到A立锯**（`CheckDispatcherSortingSafetyToA`）：
- A龙门夹臂在原点(D2174/D2176=1500±5) 且 D2046=0±5mm

**送料到B立锯**（`CheckDispatcherSortingSafetyToB`）：
- A龙门安全检查（二选一）：
  - D2046 ≥ D2188-5（实时坐标≥静态坐标-容差）
  - 夹臂在原点 + D2046=0±5
- B龙门安全检查：D2062=0±5 且 D2178/D2180=1500±5
- 安全位不满足时，自动写入D2194等待位+M107暂停

### 6.2 送料执行

```
安全位检查通过
  → 写入D2044目标坐标(A=4000 / B=8522)
  → 写入M59=true(启动送料)
  → 持续监测D2042回读值,等待D2042=D2044±0.1
  → 设置dispatcherMoving=false
  → 推送LED面板状态
```

> 回读验证超时300秒，失败时调用 `rollbackSawAssignment()` 回滚立锯分配状态

### 6.3 分配回滚机制

当D2044写入失败、M59写入失败或回读超时时，执行回滚：
1. 清除 `SawProcessor.currentWoodID`（立锯置闲）
2. DB中 `process_status` 恢复为未加工(0)，`saw_type` 清空

---

## 7. 等待位流程

### 7.1 进入等待位（逻辑6）

当A/B立锯都繁忙时：
```
写入D2194=2300(A等待位坐标)
  → 写入M107=true(启动等待位)
  → dispatcherMoving=true
  → 推送LED面板(A/B都繁忙)
  → 监控调度车到达等待位
```

### 7.2 A等待位有料检测（逻辑7）

```
D2042=2300±5mm(到达A等待位)
  && M52=true(调度车有料)
  → 上升沿触发(只执行一次)
  → dispatcherMoving=false
  → startSawIdleMonitoring()启动空闲轮询
```

### 7.3 立锯空闲轮询（逻辑8）

> 双重检查锁 + goroutine防泄漏设计

```
startSawIdleMonitoring():
  ├─ 检查pendingSawTaskCount > 0
  ├─ 并行启动:
  │   ├─ monitorSawAIdleAndDispatch()
  │   └─ monitorSawBIdleAndDispatch()
  └─ 任务完成后回到检查,计数=0时退出

monitorSawAIdleAndDispatch():
  A空闲 && B空闲 → 优先B,A退出
  A空闲 && B忙   → 查询DB,分配到A立锯
                    → gantryADispatchTrigger通道通知龙门

monitorSawBIdleAndDispatch():
  B空闲 → 查询DB,分配到B立锯
          → gantryBDispatchTrigger通道通知龙门
```

---

## 8. A/B立锯协调逻辑

### 8.1 B立锯优先完成调度（逻辑9）

当调度车在A等待位(D2042=2300)且有料(M52=true)，A/B立锯都忙时：

```
实时对比A/B立锯加工进度比例:
  sawBProgress = processedCoords / totalCoords (B锯)
  sawAProgress = processedCoords / totalCoords (A锯)

  当 sawBProgress > sawAProgress + 0.001:
    → B立锯将优先完成
    → CheckDispatcherSortingSafetyToB(非阻塞A龙门检查)
    → 写入B等待位坐标,调度车提前移动到B等待位
```

### 8.2 B等待位有料监控（逻辑10）

```
M52=true(调度车有料)
  && D2042=6208±5mm(到达B等待位)
  → 检查龙门安全位(B优先,再尝试A)
  → 写入D2044目标坐标 + M59送料
  → M52下降沿重置防重复标志
```

---

## 9. 龙门取料与入锯

送料调度车到达立锯位置后，龙门执行取料→入锯→加工流程。

### 9.1 龙门取料触发

```
A龙门: D2042=4000±0.5 + M52上升沿 → executeGantryAPickup()
B龙门: D2042=8522±0.5 + M52上升沿 → executeGantryBPickup()
```

### 9.2 龙门取料流程（以A龙门为例）

```
1. 红外测距仪写入夹臂顶料位置坐标
2. 调度车升到预定高度(D2196, M800启动, M801完成)
   公式: 820 + (-18 - max(D2182, D2184)) + D2172
3. M67/M85启动取料
4. M68/M86上升沿确认取料完成
5. 计算龙门入锯坐标: 1500 - 左夹臂当前位置 → D2186/D2190
6. 调度车高度降到0(D2196=0)
7. M73/M91龙门入锯
8. D2194=2300 + M107调度车回等待位
9. 异步监控M108到达等待位,触发立锯调度(triggerDispatch=true)
```

---

## 10. 立锯加工流程

### 10.1 触发条件

```
A立锯: M74上升沿 && M68=true(启用)
B立锯: M92上升沿 && M86=true(启用)
```

### 10.2 加工流程（每次M74/M92上升沿执行一轮）

```
1. 数据加载: 从DB加载原木left/right坐标数组
2. 坐标写入: 
   left  → D2014 + M108 → 等待M109上升沿(调整完成)
   right → D2016 + M110 → 等待M111上升沿(调整完成)
3. 切割触发: M75/M93启动切割
4. 切割完成: M76/M94上升沿 → M112/M114锯片避让 → 龙门回退
5. 循环: currentIndex++, 处理下一组坐标
6. 全部完成: handleCompletion()
   → 更新DB process_status=已完成(2)
   → 清除currentWoodID(立锯置闲)
   → M71/M89龙门回原 → M72/M90确认
```

> 坐标修正：坐标值减去最小间距的一半(8.9/2=4.45cm)后写入PLC，防止双锯碰撞

---

## 11. 寄存器总表

### 11.1 送料调度车

| 寄存器 | 类型 | 说明 |
|--------|------|------|
| D2042 | D(读) | 送料调度车当前坐标 |
| D2044 | D(写) | 送料调度车目标坐标 |
| D2194 | D(写) | 等待位坐标写入 |
| M46 | M(读) | 视觉顶针旋转完成 |
| M51 | M(读) | 送料调度车空闲 |
| M52 | M(读) | 送料调度车有料 |
| M57 | M(写) | 送料调度车取料启动 |
| M58 | M(读) | 送料调度车取料完成 |
| M59 | M(写) | 送料调度车送料启动 |
| M107 | M(写) | 送料调度车等待位启动 |
| M108 | M(读) | 送料调度车到达等待位 |
| M117 | M(写) | 视觉顶针回退 |
| M118 | M(读) | 视觉顶针回退完成 |
| M121 | M(写) | 相机扫描架回退 |
| M122 | M(读) | 相机扫描架回退完成 |

### 11.2 升降调度车

| 寄存器 | 类型 | 说明 |
|--------|------|------|
| D2130 | D(读) | 升降车实时坐标 |
| D2200 | D(写) | 升降车目标位置 |
| X56 | X(读) | 升降台回原位感应 |
| X72 | X(读) | 升降调度车有料检测 |
| M25 | M(写) | 升降车上升顶料 |
| M41 | M(读) | 视觉顶针空闲状态 |
| M808 | M(写) | 启动送料 |

### 11.3 龙门(A/B)

| 寄存器 | A龙门 | B龙门 | 说明 |
|--------|-------|-------|------|
| 实时坐标 | D2046 | D2062 | 龙门轨道坐标 |
| 静态坐标 | D2188 | D2192 | 龙门静态坐标 |
| 夹臂左位 | D2174 | D2178 | 左夹臂坐标 |
| 夹臂右位 | D2176 | D2180 | 右夹臂坐标 |
| 入锯坐标 | D2186 | D2190 | 龙门入锯坐标 |
| 取料启动 | M67 | M85 | 龙门取料指令 |
| 取料完成 | M68 | M86 | 取料完成确认 |
| 入锯指令 | M73 | M91 | 龙门入锯 |
| 回原指令 | M71 | M89 | 龙门回原 |
| 回原确认 | M72 | M90 | 回原完成 |

### 11.4 立锯(A/B)

| 寄存器 | A立锯 | B立锯 | 说明 |
|--------|-------|-------|------|
| 监控触发 | M74 | M92 | 立锯监控寄存器 |
| 启用状态 | M68 | M86 | 立锯启用 |
| 切割启动 | M75 | M93 | 启动切割 |
| 切割完成 | M76 | M94 | 切割完成上升沿 |
| left坐标 | D2014 | D2014 | 立锯left坐标值 |
| right坐标 | D2016 | D2016 | 立锯right坐标值 |

---

## 12. 关键坐标常量

| 常量 | 值(mm) | 说明 |
|------|--------|------|
| GantryATargetPosition | 4000 | A立锯取料绝对坐标 |
| GantryBTargetPosition | 8522 | B立锯取料绝对坐标 |
| DispatcherAWaitPosition | 2300 | A点等待位坐标 |
| DispatcherBWaitPosition | 6208 | B点等待位坐标 |
| VisionPhotoPosition | 2630 | 视觉拍照位置 |
| LiftWaitPosition | 1100 | 升降车等待位 |
| DispatcherPositionTolerance | 0.1 | 调度车到位容差 |
| PositionTolerance2 | 0.5 | 位置容差(宽) |
| LeftClampOriginPosition | 1500 | 左夹臂原点位置 |
| DispatcherBaseHeight | 820 | 调度车基准高度 |

---

## 13. 并发与安全机制

### 13.1 通道驱动

| 通道 | 触发方 | 消费方 | 说明 |
|------|--------|--------|------|
| sawDispatchTrigger | 视觉扫描完成 | monitorSawDispatchTrigger | 调度任务入口 |
| gantryADispatchTrigger | 立锯空闲轮询 | 龙门调度模块 | A龙门送料通知 |
| gantryBDispatchTrigger | 立锯空闲轮询 | 龙门调度模块 | B龙门送料通知 |

### 13.2 任务计数管理

- `pendingSawTaskCount`（atomic int32）：管理待处理任务数
- +1时机：sawDispatchTrigger收到事件
- -1时机：分配成功、查询失败、超时、M107写入失败
- 目的：防止任务泄漏和goroutine无限堆积

### 13.3 安全标志

| 标志 | 作用 |
|------|------|
| visionProcessing | 视觉处理中禁止取料，防止视觉设备运动中调度车动作 |
| m57WriteExecuted | 防止M57重复写入 |
| bWaitPosFeedExecuted | 防止B等待位送料重复执行，M52下降沿重置 |
| dispatcherMoving | 调度车运动中标志，取料/送料时置true，到位后置false |
| isMonitoringSawIdle | 防止轮询goroutine泄漏 |

### 13.4 超时保护

| 场景 | 超时时间 | 处理 |
|------|---------|------|
| M58取料完成检测 | 3600秒 | 减少计数器，记录异常 |
| M118/M122回退完成 | 30秒 | 记录异常，停止流程 |
| D2042回读验证 | 300秒 | 回滚立锯分配 |
| 等待位到达检测 | 3600秒 | 减少计数器，记录异常 |
| M108到达等待位 | 30秒 | 强制清除运动标记 |