init

2026-06-08 17:52:48 +08:00
commit 7a15bd8e14
189 changed files with 192687 additions and 0 deletions
@@ -0,0 +1,113 @@
+from dataclasses import dataclass
+import time
+
+from .filters import ExponentialFilter
+
+
+def shear_magnitude(sample):
+    return (float(sample["fshearx"]) ** 2 + float(sample["fsheary"]) ** 2) ** 0.5
+
+
+@dataclass
+class ForceBaseline:
+    left_normal: float = 0.0
+    right_normal: float = 0.0
+    left_shear: float = 0.0
+    right_shear: float = 0.0
+
+
+@dataclass
+class ForceFeedback:
+    left_normal: float
+    right_normal: float
+    min_normal: float
+    max_normal: float
+    normal_diff: float
+    left_shear: float
+    right_shear: float
+    max_shear: float
+    shear_diff: float
+
+
+class TactileFeedbackProcessor:
+    def __init__(self, config, normal_converter, shear_converter):
+        self.config = config
+        self.normal_converter = normal_converter
+        self.shear_converter = shear_converter
+        self.baseline = ForceBaseline()
+        self.reset_filters()
+
+    def reset_filters(self):
+        self.left_normal_filter = ExponentialFilter(self.config.filter_alpha)
+        self.right_normal_filter = ExponentialFilter(self.config.filter_alpha)
+        self.left_shear_filter = ExponentialFilter(self.config.shear_filter_alpha)
+        self.right_shear_filter = ExponentialFilter(self.config.shear_filter_alpha)
+
+    def calibrate_baseline(self, reader):
+        seconds = self.config.baseline_seconds
+        if seconds <= 0:
+            self.baseline = ForceBaseline()
+            return self.baseline
+
+        left_values = []
+        right_values = []
+        left_shear_values = []
+        right_shear_values = []
+        deadline = time.perf_counter() + seconds
+        while time.perf_counter() < deadline:
+            left, right = reader.get_samples(timeout=0.2)
+            if left is not None:
+                left_values.append(self.normal_converter.convert(left["fnormal"]))
+                left_shear_values.append(self.shear_converter.convert(shear_magnitude(left)))
+            if right is not None:
+                right_values.append(self.normal_converter.convert(right["fnormal"]))
+                right_shear_values.append(self.shear_converter.convert(shear_magnitude(right)))
+            time.sleep(0.01)
+
+        self.baseline = ForceBaseline(
+            left_normal=sum(left_values) / len(left_values) if left_values else 0.0,
+            right_normal=sum(right_values) / len(right_values) if right_values else 0.0,
+            left_shear=sum(left_shear_values) / len(left_shear_values) if left_shear_values else 0.0,
+            right_shear=sum(right_shear_values) / len(right_shear_values) if right_shear_values else 0.0,
+        )
+        return self.baseline
+
+    def read(self, reader, timeout=0.3):
+        left, right = reader.get_samples(timeout=timeout)
+        if left is None or right is None:
+            return None
+
+        left_force = max(
+            0.0,
+            self.normal_converter.convert(left["fnormal"]) - self.baseline.left_normal,
+        )
+        right_force = max(
+            0.0,
+            self.normal_converter.convert(right["fnormal"]) - self.baseline.right_normal,
+        )
+        left_shear = max(
+            0.0,
+            self.shear_converter.convert(shear_magnitude(left)) - self.baseline.left_shear,
+        )
+        right_shear = max(
+            0.0,
+            self.shear_converter.convert(shear_magnitude(right)) - self.baseline.right_shear,
+        )
+
+        left_normal = self.left_normal_filter.update(left_force)
+        right_normal = self.right_normal_filter.update(right_force)
+        left_shear_filtered = self.left_shear_filter.update(left_shear)
+        right_shear_filtered = self.right_shear_filter.update(right_shear)
+
+        return ForceFeedback(
+            left_normal=left_normal,
+            right_normal=right_normal,
+            min_normal=min(left_normal, right_normal),
+            max_normal=max(left_normal, right_normal),
+            normal_diff=left_normal - right_normal,
+            left_shear=left_shear_filtered,
+            right_shear=right_shear_filtered,
+            max_shear=max(left_shear_filtered, right_shear_filtered),
+            shear_diff=left_shear_filtered - right_shear_filtered,
+        )
+