當你理解「增材」的那一刻，
你才真正離開了「製造」的舊世界。

/* Mega 2560 · 16 路 ADC · A0–A15 分區映射
 * A0       拇指尖光纖（class 0）
 * A1–A2    拇指後電容（class 1）
 * A3–A6    4 指指腹 FSR
 * A7–A15   掌心 3×3 電容陣列
 * 取樣率：每通道 500 Hz · 總頻寬 15 × 500 = 7.5 kSPS */

#define N_CH 15
const int PINS[N_CH] = {A0,A1,A2,A3,A4,A5,A6,A7,A8,A9,A10,A11,A12,A13,A14};
int   val[N_CH], prev[N_CH];
long  lastT;

void setup() {
  Serial.begin(115200);
  // 加速 ADC：prescaler 128 → 16（提速 4 倍至 ~28 μs/次）
  ADCSRA = (ADCSRA & 0xF8) | 0x04;
  // 校正：上電 1 秒後靜止值當 baseline
  delay(1000);
  for (int i=0; i<N_CH; i++) prev[i] = analogRead(PINS[i]);
}

void loop() {
  if (micros() - lastT < 2000) return;    // 500 Hz 節流
  lastT = micros();

  bool anyChange = false;
  for (int i=0; i<N_CH; i++) {
    val[i] = analogRead(PINS[i]);
    // Δ > 10 LSB 才算有意義變化（雜訊過濾）
    if (abs(val[i] - prev[i]) > 10) {
      anyChange = true;
      prev[i] = val[i];
    }
  }

  if (anyChange) {
    Serial.print("T "); Serial.print(millis());
    for (int i=0; i<N_CH; i++) {
      Serial.print(" "); Serial.print(val[i]);
    }
    Serial.println();
  }
}

"""PID 握力控制 · ROS2 節點
   訂閱 /hand/fsr_array (Float32MultiArray, 15 ch)
   發佈 /hand/servo_cmd  (Float32MultiArray, 7 軸)
   目標：維持每指力量 2.0 N ± 0.3 N"""

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32MultiArray

class GraspPID(Node):
    def __init__(self):
        super().__init__("grasp_pid")
        self.target_N = 2.0
        self.Kp, self.Ki, self.Kd = 0.40, 0.05, 0.08
        self.err_int = [0.0] * 5
        self.err_prev = [0.0] * 5
        self.servo_pos = [90.0] * 7

        self.create_subscription(Float32MultiArray, "/hand/fsr_array", self.cb, 10)
        self.pub = self.create_publisher(Float32MultiArray, "/hand/servo_cmd", 10)

    def fsr_to_newton(self, raw):
        # FSR 映射：0–1023 → 0–10 N（線性近似）
        return raw / 102.3

    def cb(self, msg):
        force = [self.fsr_to_newton(msg.data[i+3]) for i in range(5)]  # ch 3-7
        for i in range(5):
            err = self.target_N - force[i]
            self.err_int[i] += err * 0.002          # dt = 2 ms
            derr = (err - self.err_prev[i]) / 0.002
            dp = self.Kp * err + self.Ki * self.err_int[i] + self.Kd * derr
            self.servo_pos[i] = max(0, min(180, self.servo_pos[i] + dp))
            self.err_prev[i] = err

        cmd = Float32MultiArray()
        cmd.data = self.servo_pos
        self.pub.publish(cmd)

def main():
    rclpy.init()
    node = GraspPID()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()

# Hand Test Report · v0.3 · 2026-04-15

## 受測品
- Rev: B2 · BOM: 4,580 NTD · 重量: 385 g · 驅動: Dynamixel XL-320 × 7

## 6 指標結果
| 指標    | 實測             | 門檻  | 狀態  |
|---------|------------------|-------|-------|
| 握力    | 24.5 N ± 1.2     | 15 N  | PASS  |
| 精度    | 1.8 mm (1σ)      | 3 mm  | PASS  |
| 壽命    | 12,500 cycles    | 5,000 | PASS  |
| 延遲    | 72 ms            | 100 ms| PASS  |
| 位置誤差| 4.2 mm           | 5 mm  | PASS  |
| 溫升    | 22°C             | 20°C  | **FAIL**  |

## 待改善
1. 溫升超標 · 主因伺服連續堵轉 · 需加溫度感測器做 OCP
2. 精度測試環境室溫 30°C 可能偏差 · 建議 23°C 再測

## 下版目標（Rev C）
- 溫升 < 15°C
- 握力 > 30 N
- 壽命 > 50,000 cycles

/* =============================================
 * 3D Braiding Machine V1 · 基礎定速版
 * 單一 Horn Gear 以固定 RPM 轉動
 * 配合 A4988 驅動板 + NEMA 17 步進馬達
 * ============================================= */

#include <AccelStepper.h>  // 先在 Library Manager 安裝 AccelStepper

// ===== 硬體引腳定義（對應 Day 3 接線）=====
const int STEP_PIN = 2;    // A4988 STEP → Arduino D2
const int DIR_PIN  = 3;    // A4988 DIR  → Arduino D3
const int EN_PIN   = 4;    // A4988 ENABLE → D4 (LOW=啟用)

// ===== 編織參數 =====
const int STEPS_PER_REV   = 200;   // NEMA 17 · 1.8°/step
const int MICROSTEP       = 16;    // A4988 M0/M1/M2 全接 HIGH
const float TARGET_RPM   = 20.0;  // 目標 Horn Gear 轉速（Day 7 從 20 開始）

// 計算每秒要跑多少 step
const long STEPS_PER_SEC = (long)(STEPS_PER_REV * MICROSTEP * TARGET_RPM / 60.0);

// 建立 AccelStepper 物件（驅動模式：STEP+DIR）
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

void setup() {
  Serial.begin(9600);
  pinMode(EN_PIN, OUTPUT);
  digitalWrite(EN_PIN, LOW);      // 啟用 A4988

  stepper.setMaxSpeed(STEPS_PER_SEC * 2);
  stepper.setAcceleration(STEPS_PER_SEC);  // 1 秒達速
  stepper.setSpeed(STEPS_PER_SEC);         // 定速模式

  Serial.print("Braiding V1 start | steps/sec = ");
  Serial.println(STEPS_PER_SEC);
}

void loop() {
  stepper.runSpeed();   // 非阻塞 · 持續轉動
  // TODO V2: 加張力感測器回饋 · 若張力過高減速
}

/* =============================================
 * 3D Braiding Machine V2 · 多軸同步 + 張力閉環
 * 4× A4988 + 4× NEMA 17 (1.8°/step, 200 steps/rev)
 * 1× HX711 + 應變片 (量程 5 kg)
 * Serial 命令：rpm N / pause / resume / tension
 * ============================================= */

// 安裝：Arduino IDE → Tools → Manage Libraries →
// 搜尋安裝：AccelStepper (Mike McCauley) + HX711 (Bogdan Necula)
#include <AccelStepper.h>
#include <HX711.h>          // 張力感測

// ===== 引腳（對應 Day 10 接線） =====
const int STEP_PINS[4] = { 2, 4, 6, 8  };
const int DIR_PINS [4] = { 3, 5, 7, 9  };
const int EN_PIN        = 10;     // 所有 A4988 共用 EN
const int HX_DOUT       = A1;
const int HX_SCK        = A0;

// ===== 編織參數 =====
const int STEPS_PER_REV = 200;
const int MICROSTEP     = 16;
float     targetRPM     = 20.0;       // 可被 Serial 覆寫
const float TENSION_MAX = 2500.0;    // 2.5 kg 警戒線
const float TENSION_MIN = 300.0;     // 太鬆表示斷紗

// ===== 物件 =====
AccelStepper steppers[4] = {
  {AccelStepper::DRIVER, STEP_PINS[0], DIR_PINS[0]},
  {AccelStepper::DRIVER, STEP_PINS[1], DIR_PINS[1]},
  {AccelStepper::DRIVER, STEP_PINS[2], DIR_PINS[2]},
  {AccelStepper::DRIVER, STEP_PINS[3], DIR_PINS[3]}
};
HX711        scale;

bool  running  = true;
long  lastLog  = 0;

// ===== 校正（重要！）=====
// 第一次燒錄後先把 scale.set_scale 註解掉 · 跑 tare() 後秤一個已知
// 500 g 砝碼 · 把原始讀值 / 500 就是校正係數 · 再回填到下一行
const float HX_CALIB = 220.5;   // ← 換成你實測值

long rpmToStepsPerSec(float rpm) {
  return (long)(STEPS_PER_REV * MICROSTEP * rpm / 60.0);
}

void setSpeedAll(float rpm) {
  long s = rpmToStepsPerSec(rpm);
  for (int i = 0; i < 4; i++) {
    steppers[i].setMaxSpeed(s * 2);
    steppers[i].setAcceleration(s);
    // 交錯方向：0,2 正轉 · 1,3 反轉 · 產生 Maypole 交織
    steppers[i].setSpeed((i % 2 == 0) ? s : -s);
  }
}

void handleCommand(String cmd) {
  cmd.trim();
  if (cmd.startsWith("rpm ")) {
    targetRPM = cmd.substring(4).toFloat();
    setSpeedAll(targetRPM);
    Serial.print("RPM -> "); Serial.println(targetRPM);
  } else if (cmd == "pause") {
    running = false; digitalWrite(EN_PIN, HIGH);
    Serial.println("PAUSED");
  } else if (cmd == "resume") {
    running = true;  digitalWrite(EN_PIN, LOW);
    Serial.println("RESUMED");
  } else if (cmd == "tension") {
    Serial.print("TENSION ");
    Serial.print(scale.get_units(5));
    Serial.println(" g");
  } else if (cmd == "tare") {
    scale.tare();              // 歸零 · 換紗後必跑
    Serial.println("TARED");
  }
}

void setup() {
  Serial.begin(9600);
  pinMode(EN_PIN, OUTPUT);
  digitalWrite(EN_PIN, LOW);

  scale.begin(HX_DOUT, HX_SCK);
  scale.set_scale(HX_CALIB);
  scale.tare();                    // 開機自動歸零

  setSpeedAll(targetRPM);
  Serial.println("Braiding V2 READY · cmd: rpm|pause|resume|tension|tare");
}

void loop() {
  // 1. 命令解析（非阻塞）
  if (Serial.available()) {
    handleCommand(Serial.readStringUntil('\n'));
  }

  // 2. 張力監控（每 200 ms 一次 · 不佔主迴圈）
  if (millis() - lastLog > 200) {
    lastLog = millis();
    float g = scale.get_units(1);    // 1 次取樣 · 減少延遲
    if (g > TENSION_MAX) {
      setSpeedAll(targetRPM * 0.5);       // 張力爆表 · 降速保護紗
      Serial.println("! HIGH TENSION · slow down");
    } else if (g < TENSION_MIN) {
      running = false;                    // 斷紗 · 立即停
      digitalWrite(EN_PIN, HIGH);
      Serial.println("!! LINE BROKEN · halted");
    }
  }

  // 3. 4 軸定速運轉 · 每 stepper 有獨立 runSpeed 但共用主 loop
  //    這樣已達「視覺同步」效果（60 rpm 差異 < 1 step / 迴圈）
  if (running) {
    for (int i = 0; i < 4; i++) steppers[i].runSpeed();
  }
}

"""
Braiding Path Planner
輸入：二維花紋矩陣（每格 0/1/-1）
輸出：step sequence (JSON) · 送給 Arduino V2 執行
作者：增材之道 · MIT License
"""

import json
import argparse
from dataclasses import dataclass, asdict
from typing import List

# ===== 資料結構 =====
@dataclass
class StepCmd:
    axis:   int    # 0-3 · 對應 4 個 stepper
    steps:  int    # 正負決定轉向
    rpm:    float  # 本段速度
    reason: str    # 除錯用

# ===== Voxel 離散化 =====
def discretize(pattern, voxel_mm=2.0, height_mm=50.0):
    """把 2D 花紋沿 Z 軸堆成 3D voxel · 每層對應一組 stepper 動作"""
    rows, cols = len(pattern), len(pattern[0])
    layers = int(height_mm / voxel_mm)
    voxel_grid = []
    for z in range(layers):
        layer = [[pattern[y][x] for x in range(cols)] for y in range(rows)]
        voxel_grid.append(layer)
    return voxel_grid

# ===== 核心規劃器 =====
def plan(pattern, rpm=20, steps_per_rev=200, microstep=16, carriers=4):
    """核心：把 voxel grid 轉為 StepCmd list
    規則：
      1 → 軸正轉一個 voxel 週期
     -1 → 軸反轉一個 voxel 週期
      0 → 該軸 idle（其他軸繼續）
    每個 voxel 週期 = steps_per_voxel 步
    """
    grid = discretize(pattern)
    steps_per_voxel = steps_per_rev * microstep // 8   # 1 voxel = 45°
    cmds: List[StepCmd] = []

    for z, layer in enumerate(grid):
        for y, row in enumerate(layer):
            for x, v in enumerate(row):
                axis = (x + y) % carriers          # 花紋→軸映射
                if v == 0: continue            # 跳過閒置格
                cmds.append(StepCmd(
                    axis=axis,
                    steps=v * steps_per_voxel,
                    rpm=rpm,
                    reason=f"z{z} y{y} x{x} v{v}"
                ))
    return cmds

# ===== 序列化（給 Arduino）=====
def to_arduino_json(cmds):
    """Arduino V3 會接收這個 JSON · 用 ArduinoJson library 解析"""
    return json.dumps({
        "version": "1.0",
        "total_cmds": len(cmds),
        "cmds": [asdict(c) for c in cmds]
    }, indent=2)

# ===== 範例：2×2 交叉花紋 =====
if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("--rpm", type=float, default=20)
    parser.add_argument("--out", default="plan.json")
    args = parser.parse_args()

    demo_pattern = [
        [1, -1,  0,  1],
        [-1,  1,  1, -1],
        [0,  1, -1,  1],
        [1, -1,  1, -1]
    ]
    cmds = plan(demo_pattern, rpm=args.rpm)
    out = to_arduino_json(cmds)
    with open(args.out, "w") as f:
        f.write(out)
    print(f"產出 {len(cmds)} 條指令 → {args.out}")

/* 最小可用 G-code 解析器 · 支援本頁 15 條指令
 * 把整行字串切成 token · switch 分派
 * 搭配 V2 的 setSpeedAll/handleCommand 可直接工作 */

void parseLine(String line) {
  line.trim();
  if (line.length() == 0 || line.startsWith(";")) return; // 空行/註解

  String code = line.substring(0, 3);     // "G01" / "M03" / "T8 "
  float F = extractValue(line, 'F');      // feed rate (rpm)
  float S = extractValue(line, 'S');      // spindle speed
  float P = extractValue(line, 'P');      // pause ms
  float R = extractValue(line, 'R');      // rotation count
  float T = extractValue(line, 'T');      // tension / carriers

  if (code == "G00") { emergencyStop(); }
  else if (code == "G01") { setSpeedAll(F); runLayers(1); }
  else if (code == "G02") { setSpeedAll(F);  runLayers((int)R); }    // 正螺旋
  else if (code == "G03") { setSpeedAll(-F); runLayers((int)R); }    // 反螺旋
  else if (code == "G04") { delay((int)P); }
  else if (code == "G28") { homeAll(); }
  else if (code == "M00") { while(!buttonPressed()) delay(10); }
  else if (code == "M03") { setSpeedAll(S); running = true; }
  else if (code == "M05") { running = false; }
  else if (code == "M09") { TENSION_MAX_LIVE = T; }
  else if (code == "M30") { running = false; Serial.println("DONE"); }
  else if (code.startsWith("T")) { activeCarriers = (int)T; }
  else if (code.startsWith("W")) { runLayers((int)extractValue(line, 'W')); }
}

float extractValue(String line, char key) {
  int i = line.indexOf(key);
  if (i < 0) return 0;
  int end = line.indexOf(' ', i);
  if (end < 0) end = line.length();
  return line.substring(i + 1, end).toFloat();
}

void runLayers(int n) {
  long steps_per_layer = STEPS_PER_REV * MICROSTEP;
  for (int L = 0; L < n; L++) {
    long start = steppers[0].currentPosition();
    while (abs(steppers[0].currentPosition() - start) < steps_per_layer) {
      for (int i = 0; i < 4; i++) steppers[i].runSpeed();
    }
  }
}