cu_feetech/
calibration.rs

1//! Calibration data and unit conversions for Feetech servos.
2//!
3//! Each servo has a recorded min and max raw position. The center is
4//! `(min + max) / 2` and is used as the zero reference when converting
5//! to degrees or radians.
6//!
7//! Run the `feetech-calibrate` binary to generate a `calibration.json`.
8
9use cu29::units::si::angle::{degree, radian};
10use cu29::units::si::f32::Angle;
11use serde::{Deserialize, Serialize};
12use std::path::Path;
13use std::str::FromStr;
14
15/// Output unit for published positions.
16#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
17pub enum Units {
18    /// Raw 16-bit register values (0–65535).  No calibration needed.
19    #[default]
20    Raw,
21    /// Degrees relative to the calibration center (0 = center).
22    Deg,
23    /// Radians relative to the calibration center (0 = center).
24    Rad,
25    /// Normalized range [-1, 1]: min → -1, center → 0, max → 1. Same scale for leader/follower.
26    Normalize,
27}
28
29impl FromStr for Units {
30    type Err = ();
31
32    /// Parse from a config string.  Returns `Err` for unrecognised values.
33    fn from_str(s: &str) -> Result<Self, Self::Err> {
34        match s {
35            "raw" => Ok(Self::Raw),
36            "deg" => Ok(Self::Deg),
37            "rad" => Ok(Self::Rad),
38            "normalize" | "norm" => Ok(Self::Normalize),
39            _ => Err(()),
40        }
41    }
42}
43
44// =========================================================================
45// Conversion helpers
46// =========================================================================
47
48/// Default ticks per revolution when not specified (e.g. STS3215 often uses 4096).
49/// Actual value is model-dependent; set via bridge config `ticks_per_rev`.
50pub const DEFAULT_TICKS_PER_REV: u32 = 4096;
51
52impl Units {
53    /// Convert a raw 16-bit tick to the output unit.
54    ///
55    /// For `Raw`: `param` is ignored.
56    /// For `Deg`/`Rad`: `param` is `ticks_per_rev`.
57    /// For `Normalize`: `param` is half_range `(max - min) / 2`; result is in [-1, 1].
58    #[inline]
59    pub fn from_raw(self, raw: u16, center: f32, param: f32) -> f32 {
60        match self {
61            Self::Raw => raw as f32,
62            Self::Deg => {
63                let deg = (raw as f32 - center) * 360.0 / param;
64                Angle::new::<degree>(deg).get::<degree>()
65            }
66            Self::Rad => {
67                let rad = (raw as f32 - center) * core::f32::consts::TAU / param;
68                Angle::new::<radian>(rad).get::<radian>()
69            }
70            Self::Normalize => {
71                if param <= 0.0 {
72                    0.0
73                } else {
74                    ((raw as f32 - center) / param).clamp(-1.0, 1.0)
75                }
76            }
77        }
78    }
79
80    /// Convert an output-unit value back to a raw 16-bit tick.
81    ///
82    /// For `Normalize`, `param` is half_range; value must be in [-1, 1].
83    /// Result is clamped to `0..=65535`.
84    #[inline]
85    pub fn to_raw(self, value: f32, center: f32, param: f32) -> u16 {
86        let raw = match self {
87            Self::Raw => value,
88            Self::Deg => {
89                let deg = Angle::new::<degree>(value).get::<degree>();
90                deg * param / 360.0 + center
91            }
92            Self::Rad => {
93                let rad = Angle::new::<radian>(value).get::<radian>();
94                rad * param / core::f32::consts::TAU + center
95            }
96            Self::Normalize => center + value.clamp(-1.0, 1.0) * param,
97        };
98        raw.round().clamp(0.0, 65535.0) as u16
99    }
100}
101
102// =========================================================================
103// Per-servo calibration
104// =========================================================================
105
106/// Calibration for a single servo.
107#[derive(Debug, Clone, Serialize, Deserialize)]
108pub struct ServoCalibration {
109    pub id: u8,
110    pub min: u16,
111    pub max: u16,
112}
113
114impl ServoCalibration {
115    /// Midpoint between min and max — the "zero" position.
116    pub fn center(&self) -> f32 {
117        (self.min as f32 + self.max as f32) / 2.0
118    }
119
120    /// Total usable range in raw ticks.
121    pub fn range(&self) -> u16 {
122        self.max.saturating_sub(self.min)
123    }
124}
125
126/// Calibration data for all servos on a bus.
127#[derive(Debug, Clone, Serialize, Deserialize, Default)]
128pub struct CalibrationData {
129    pub servos: Vec<ServoCalibration>,
130}
131
132impl CalibrationData {
133    pub fn load(path: &Path) -> std::io::Result<Self> {
134        let contents = std::fs::read_to_string(path)?;
135        serde_json::from_str(&contents)
136            .map_err(|e| std::io::Error::other(format!("bad calibration JSON: {e}")))
137    }
138
139    pub fn save(&self, path: &Path) -> std::io::Result<()> {
140        let json = serde_json::to_string_pretty(self)?;
141        std::fs::write(path, json)
142    }
143
144    /// Look up the center (midpoint) for a servo by bus ID.
145    ///
146    /// Returns `None` if no calibration entry exists for that ID.
147    pub fn center_for(&self, id: u8) -> Option<f32> {
148        self.servos.iter().find(|s| s.id == id).map(|s| s.center())
149    }
150
151    /// Look up half the range `(max - min) / 2` for a servo by bus ID.
152    /// Used for the `normalize` unit ([-1, 1] over the calibrated range).
153    pub fn half_range_for(&self, id: u8) -> Option<f32> {
154        self.servos
155            .iter()
156            .find(|s| s.id == id)
157            .map(|s| (s.max as f32 - s.min as f32) / 2.0)
158    }
159}
cu_feetech/calibration.rs

cu_feetech/
calibration.rs
