Compare commits

...
Author SHA1 Message Date
Icedsoda 69a3cdbef3 Merge pull request #429 from qitechgmbh/428-make-adaptivespoolspeedcontroller-core-parameters-customizable
Make `AdaptiveSpoolSpeedController` Core Parameters Customizable
2025-06-28 17:23:49 +02:00
Icedsoda ff51018d35 Make AdaptiveSpoolSpeedController Core Parameters Customizable 2025-06-28 17:22:18 +02:00
Icedsoda a228906eed Merge pull request #427 from qitechgmbh/426-show-estimated-spool-diameter
Show estimated diameter
2025-06-28 16:49:29 +02:00
Icedsoda a238fc9246 Show estimated diameter 2025-06-28 16:48:00 +02:00
Icedsoda 82a7e2fbfa Merge pull request #401 from qitechgmbh/388-improve-spool-speed-controller-again
Improve spool speed controller (again)
2025-06-28 15:58:33 +02:00
Icedsoda 1d78bacbe8 fix test errors 2025-06-28 14:36:40 +02:00
Icedsoda 043e781049 Improve AdaptiveSpoolSpeedController 2025-06-28 14:36:40 +02:00
Icedsoda 2d23d9db2f Make two spool speed algos available 2025-06-28 14:36:40 +02:00
18 changed files with 2310 additions and 249 deletions

No files matched your search

Generated
+1
View File
@@ -592,6 +592,7 @@ dependencies = [
"serialport",
"smol",
"socketioxide",
"textplots",
"tracing",
"uom",
]
+1
View File
@@ -22,3 +22,4 @@ tracing = "0.1.41"
erased-serde = "0.4.6"
[dev-dependencies]
approx = "0.5.1"
textplots = "0.8"
+699 -5
View File
@@ -1,4 +1,26 @@
/// Normalizes a value from a given range [min, max] to the range [0, 1].
///
/// This function maps a value from an arbitrary range to the standard
/// normalized range [0, 1]. Values o/// Plot for Steepness = -1:ge are clamped
/// to the boundaries (0.0 or 1.0).
///
/// # Arguments
/// * `value` - The input value to normalize
/// * `min` - The minimum value of the input range
/// * `max` - The maximum value of the input range
///
/// # Returns
/// * Normalized value in range [0.0, 1.0]
///
/// # Panics
/// * Panics if `min >= max`
///
/// # Example
/// ```rust
/// use control_core::helpers::interpolation::normalize;
/// let normalized = normalize(5.0, 0.0, 10.0); // Returns 0.5
/// let clamped = normalize(-2.0, 0.0, 10.0); // Returns 0.0 (clamped)
/// ```
pub fn normalize<T>(value: T, min: T, max: T) -> f64
where
T: Into<f64>,
@@ -19,6 +41,24 @@ where
}
/// Scales a normalized value from the range [0, 1] to a new range [new_min, new_max].
///
/// This function takes a normalized value (typically from 0 to 1) and maps it
/// to a new target range. It's the inverse operation of normalization.
///
/// # Arguments
/// * `normalized_value` - Input value in range [0.0, 1.0]
/// * `new_min` - The minimum value of the target range
/// * `new_max` - The maximum value of the target range
///
/// # Returns
/// * Scaled value in the range [new_min, new_max]
///
/// # Example
/// ```rust
/// use control_core::helpers::interpolation::scale;
/// let scaled = scale(0.5, 0.0, 100.0); // Returns 50.0
/// let negative_range = scale(0.25, -10.0, 10.0); // Returns -5.0
/// ```
pub fn scale<T>(normalized_value: f64, new_min: T, new_max: T) -> T
where
T: Into<f64> + From<f64>,
@@ -31,24 +71,395 @@ where
T::from(result)
}
/// Transforma normalized value from the range [0, 1] to a [0, 1] range using an normalized exponential function.
pub fn interpolate_exponential(normalized_value: f64, a: f64) -> f64 {
if a.abs() < 1e-10 {
/// Performs hinge interpolation creating a piecewise linear function.
///
/// This function creates a piecewise linear curve that:
/// - For values < x: linearly interpolates from (0, 0) to (x, y)
/// - For values > x: linearly interpolates from (x, y) to (1, 1)
/// - For value == x: returns y
///
/// The function assumes the input value is normalized to the range [0, 1],
/// and creates a "hinge" or "elbow" shape at the point (x, y).
///
/// # Arguments
/// * `value` - The input value (typically in range [0, 1])
/// * `x` - The x-coordinate of the hinge point (typically in range [0, 1])
/// * `y` - The y-coordinate of the hinge point (typically in range [0, 1])
///
/// # Returns
/// * Interpolated value based on the hinge function
///
/// # Example
/// ```rust
/// use control_core::helpers::interpolation::interpolate_hinge;
/// // Create a hinge at (0.3, 0.7)
/// let result1 = interpolate_hinge(0.15, 0.3, 0.7); // Returns 0.35 (halfway to hinge)
/// let result2 = interpolate_hinge(0.3, 0.3, 0.7); // Returns 0.7 (at hinge)
/// let result3 = interpolate_hinge(0.65, 0.3, 0.7); // Returns 0.85 (halfway from hinge to 1)
/// ```
/// ```ignore
/// Hinge at (0.8, 0.2)
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⠁ 1.0
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡜⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠇⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠎⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡇⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⠁⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡜⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠇⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠎⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡇⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣀⠤⠤⠒⠒⠊⠁⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⣀⣀⡠⠤⠒⠒⠉⠉⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⣀⣀⠤⠤⠔⠒⠉⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠉⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
/// 0.0 1.0
/// ```
pub fn interpolate_hinge<T>(value: T, x: T, y: T) -> f64
where
T: Into<f64>,
{
let value_f64: f64 = value.into();
let x_f64: f64 = x.into();
let y_f64: f64 = y.into();
// If we are under X we scale linearly from 0 to y
// If we are over X we scale linearly from y to 1
if value_f64 < x_f64 {
return normalize(value_f64, 0.0, x_f64) * y_f64;
} else if value_f64 > x_f64 {
return normalize(value_f64, x_f64, 1.0) * (1.0 - y_f64) + y_f64;
} else {
return y_f64;
}
}
/// Transforms a normalized value from the range [0, 1] to a [0, 1] range using a normalized exponential function.
///
/// This function creates smooth curves that are either convex (fast start, slow end)
/// or concave (slow start, fast end) depending on the steepness parameter sign.
/// The curve always passes through (0,0) and (1,1).
///
/// # Arguments
/// * `normalized_value` - Input value in range [0.0, 1.0]
/// * `steepness` - Controls the curve shape:
/// - `steepness = 0.0`: Linear interpolation (y = x)
/// - `steepness > 0.0`: Convex curve (slow start → fast end)
/// - `steepness < 0.0`: Concave curve (fast start → slow end)
/// - Typical range: -5.0 to 5.0 for most applications
///
/// # Returns
/// * Transformed value in range [0.0, 1.0]
///
/// # Example
/// ```rust
/// use control_core::helpers::interpolation::interpolate_exponential;
/// let convex = interpolate_exponential(0.5, 2.0); // Slow start, fast end
/// let concave = interpolate_exponential(0.5, -2.0); // Fast start, slow end
/// let linear = interpolate_exponential(0.5, 0.0); // Linear (returns 0.5)
/// ```
///
/// # Plots
/// Steepness = -3
/// Steepness = -3
/// ```ignore
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⡠⠤⠔⠒⠒⠉⠁ 1.0
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠤⠔⠊⠉⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠒⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⡔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⢀⠎⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⡔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⡰⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⡜⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⢰⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⢀⠇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⢤⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡎⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ -0.0
/// 0.0 1.0
/// ```
///
/// Steepness = -1
/// ```ignore
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠒⠁ 1.0
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠊⠁⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠒⠁⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡰⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⢀⠜⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⡔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⢀⠎⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⡔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⢀⠜⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡲⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ -0.0
/// 0.0 1.0
/// ```
///
/// Steepness = 0
/// ```ignore
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁ 1.0
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⢂⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
/// 0.0 1.0
/// ```
///
/// Steepness = 1
/// ```ignore
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡰⠁ 1.0
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠃⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠊⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠜⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠒⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⢀⣀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⣀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⢂⣀⠔⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
/// 0.0 1.0
/// ```
///
/// Steepness = 3
/// ```ignore
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⠁ 1.0
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡎⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠎⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡜⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⠁⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠇⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡰⠁⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⠁⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⣀⠤⠔⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⣀⣀⠤⠤⠒⠒⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠉⠉⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
/// 0.0 1.0
/// ```
pub fn interpolate_exponential(normalized_value: f64, steepness: f64) -> f64 {
if steepness.abs() < 1e-10 {
// When a is very close to zero, the function becomes linear
return normalized_value;
}
let numerator = a.exp().powf(normalized_value) - 1.0;
let denominator = a.exp() - 1.0;
let numerator = steepness.exp().powf(normalized_value) - 1.0;
let denominator = steepness.exp() - 1.0;
clip(numerator / denominator)
}
/// A piecewise exponential interpolation function that creates smooth S-curves
/// through the points (0,0), (0.5,0.5), and (1,1).
///
/// The function combines two normalized exponential segments:
/// - First half (0 to 0.5): inverted exponential (concave, fast start → slow end)
/// - Second half (0.5 to 1): standard exponential (convex, slow start → fast end)
///
/// This creates an S-like curve that's concave then convex, similar to
/// a smoothed step function or sigmoid-like transition.
///
/// # Arguments
/// * `x` - Input value, should be in range [0.0, 1.0]
/// * `steepness` - Controls how steep the S-curve is
/// - `steepness = 0.0`: Linear interpolation (y = x)
/// - `steepness > 0.0`: More pronounced S-curve
/// - Higher values create sharper transitions at the boundaries
/// - Typical range: 0.5 to 5.0 for most applications
///
/// # Returns
/// * Interpolated y value in range [0.0, 1.0]
///
/// # Example
/// ```rust
/// use control_core::helpers::interpolation::interpolate_inflected_exponential;
/// let y = interpolate_inflected_exponential(0.25, 2.0); // Gentle S-curve
/// let y = interpolate_inflected_exponential(0.75, 5.0); // Sharp S-curve
/// ```
///
/// # Plots
/// Steepness = 0.5
/// ```ignore
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡰⠁ 1.0
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡰⠉⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠜⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠜⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠒⠁⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠒⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⡔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⢀⠔⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⢀⠤⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⡠⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡢⠒⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
/// 0.0 1.0
/// ```
///
/// Steepness = 1
/// ```ignore
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡰⠁ 1.0
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠒⠁⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠃⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠒⠁⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠔⠁⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠔⠒⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⡠⠒⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⢀⠤⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⡰⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⡰⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⡜⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡲⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
/// 0.0 1.0
/// ```
///
/// Steepness = 2
/// ```ignore
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⠁ 1.0
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠃⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡸⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡜⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡰⠉⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠊⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⡠⠤⠒⠊⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠔⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⣀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⢠⠒⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⡠⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⡠⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡞⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
/// 0.0 1.0
/// ```
///
/// Steepness = 5
/// ```ignore
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⠁ 1.0
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡜⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠇⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠎⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡜⠀⠀⠀
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡜⠀⠀⠀⠀
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠔⠉⠀⠀⠀⠀⠀
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⣀⣀⡠⠤⠤⠤⠤⠤⠤⠤⠒⠒⠊⠉⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⠀⠀⠀⠀⢀⠔⠒⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡁⠀⠀⢠⠒⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠄⠀⢠⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠂⢀⠇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⣁⠎⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⢼⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⡇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
/// 0.0 1.0
/// ```
pub fn interpolate_inflected_exponential(x: f64, steepness: f64) -> f64 {
debug_assert!((0.0..=1.0).contains(&x), "x must be in range [0.0, 1.0]");
debug_assert!(steepness >= 0.0, "steepness must be non-negative");
if steepness == 0.0 {
// Linear case
return x;
}
if x <= 0.5 {
// First segment: inverted exponential (concave)
// Maps [0, 0.5] → [0, 0.5] with concave shape
let x_norm = 2.0 * x; // Scale to [0, 1]
// Use inverted exponential interpolation
let result = 1.0 - interpolate_exponential(1.0 - x_norm, steepness);
0.5 * result
} else {
// Second segment: standard exponential (convex)
// Maps [0.5, 1] → [0.5, 1] with convex shape
let x_norm = 2.0 * (x - 0.5); // Scale to [0, 1]
// Use standard exponential interpolation
let result = interpolate_exponential(x_norm, steepness);
0.5 + 0.5 * result
}
}
/// Inverts a value from the range [0, 1] to [1, 0].
///
/// This function flips a normalized value, where 0 becomes 1 and 1 becomes 0.
/// Values outside [0, 1] are first clamped to this range before inversion.
///
/// # Arguments
/// * `value` - Input value (will be clamped to [0.0, 1.0])
///
/// # Returns
/// * Inverted value in range [0.0, 1.0]
///
/// # Example
/// ```rust
/// use control_core::helpers::interpolation::invert;
/// let inverted = invert(0.3); // Returns 0.7
/// let boundary = invert(0.0); // Returns 1.0
/// let clamped = invert(1.5); // Returns 0.0 (input clamped to 1.0 first)
/// ```
pub fn invert(value: f64) -> f64 {
let value = clip(value);
1.0 - value
}
/// Clips a value to the range [0, 1].
///
/// This function ensures that any input value is constrained to the
/// normalized range [0, 1] by clamping values below 0 to 0 and
/// values above 1 to 1.
///
/// # Arguments
/// * `value` - Input value to clip
///
/// # Returns
/// * Clipped value in range [0.0, 1.0]
///
/// # Example
/// ```rust
/// use control_core::helpers::interpolation::clip;
/// let normal = clip(0.5); // Returns 0.5
/// let low = clip(-0.3); // Returns 0.0
/// let high = clip(1.7); // Returns 1.0
/// ```
pub fn clip(value: f64) -> f64 {
if value < 0.0 {
return 0.0;
@@ -178,6 +589,289 @@ mod tests {
assert_relative_eq!(clip(1.5), 1.0, epsilon = EPSILON);
}
#[test]
fn test_interpolate_inflected_exponential() {
// Test boundary points (0,0), (0.5,0.5), and (1,1) for various steepness values
for steepness in [0.0, 0.5, 1.0, 2.0, 5.0].iter() {
assert_relative_eq!(
interpolate_inflected_exponential(0.0, *steepness),
0.0,
epsilon = EPSILON
);
assert_relative_eq!(
interpolate_inflected_exponential(0.5, *steepness),
0.5,
epsilon = EPSILON
);
assert_relative_eq!(
interpolate_inflected_exponential(1.0, *steepness),
1.0,
epsilon = EPSILON
);
}
// Test linear case (steepness = 0)
for x in [0.0, 0.25, 0.5, 0.75, 1.0].iter() {
assert_relative_eq!(
interpolate_inflected_exponential(*x, 0.0),
*x,
epsilon = EPSILON
);
}
// Test monotonicity - function should be strictly increasing
for steepness in [0.5, 1.0, 2.0, 5.0].iter() {
let mut prev = interpolate_inflected_exponential(0.0, *steepness);
for i in 1..=20 {
let x = i as f64 / 20.0;
let current = interpolate_inflected_exponential(x, *steepness);
assert!(
current >= prev,
"Failed monotonicity test at x={}, steepness={}, prev={}, current={}",
x,
steepness,
prev,
current
);
prev = current;
}
}
// Test S-curve property: first half should be concave, second half convex
let steepness = 2.0;
// First half: check concave property (derivative decreasing)
let x1 = 0.1;
let x2 = 0.2;
let x3 = 0.3;
let y1 = interpolate_inflected_exponential(x1, steepness);
let y2 = interpolate_inflected_exponential(x2, steepness);
let y3 = interpolate_inflected_exponential(x3, steepness);
let slope1 = (y2 - y1) / (x2 - x1);
let slope2 = (y3 - y2) / (x3 - x2);
assert!(
slope1 > slope2,
"First half should be concave (decreasing slope)"
);
// Second half: check convex property (derivative increasing)
let x1 = 0.7;
let x2 = 0.8;
let x3 = 0.9;
let y1 = interpolate_inflected_exponential(x1, steepness);
let y2 = interpolate_inflected_exponential(x2, steepness);
let y3 = interpolate_inflected_exponential(x3, steepness);
let slope1 = (y2 - y1) / (x2 - x1);
let slope2 = (y3 - y2) / (x3 - x2);
assert!(
slope1 < slope2,
"Second half should be convex (increasing slope)"
);
// Test that output is always in [0, 1] range
for steepness in [0.0, 1.0, 5.0, 10.0].iter() {
for i in 0..=100 {
let x = i as f64 / 100.0;
let y = interpolate_inflected_exponential(x, *steepness);
assert!(
y >= 0.0 && y <= 1.0,
"Output {} out of range [0,1] for x={}, steepness={}",
y,
x,
steepness
);
}
}
}
#[test]
fn test_interpolate_hinge() {
// Test basic functionality with hinge at (0.3, 0.7)
let x_hinge = 0.3;
let y_hinge = 0.7;
// Test exact hinge point
assert_relative_eq!(
interpolate_hinge(x_hinge, x_hinge, y_hinge),
y_hinge,
epsilon = EPSILON
);
// Test left segment (value < x): should interpolate from (0,0) to (x,y)
assert_relative_eq!(
interpolate_hinge(0.0, x_hinge, y_hinge),
0.0,
epsilon = EPSILON
);
assert_relative_eq!(
interpolate_hinge(0.15, x_hinge, y_hinge),
0.35, // 0.15/0.3 * 0.7 = 0.5 * 0.7 = 0.35
epsilon = EPSILON
);
// Test right segment (value > x): should interpolate from (x,y) to (1,1)
assert_relative_eq!(
interpolate_hinge(1.0, x_hinge, y_hinge),
1.0,
epsilon = EPSILON
);
assert_relative_eq!(
interpolate_hinge(0.65, x_hinge, y_hinge),
0.85, // y + ((0.65-0.3)/(1.0-0.3)) * (1.0-0.7) = 0.7 + 0.5 * 0.3 = 0.85
epsilon = EPSILON
);
// Test with different hinge points
// Hinge at (0.5, 0.5) - should be linear
assert_relative_eq!(interpolate_hinge(0.25, 0.5, 0.5), 0.25, epsilon = EPSILON);
assert_relative_eq!(interpolate_hinge(0.75, 0.5, 0.5), 0.75, epsilon = EPSILON);
// Hinge at (0.2, 0.9) - steep left, shallow right
assert_relative_eq!(
interpolate_hinge(0.1, 0.2, 0.9),
0.45, // 0.1/0.2 * 0.9 = 0.5 * 0.9 = 0.45
epsilon = EPSILON
);
assert_relative_eq!(
interpolate_hinge(0.6, 0.2, 0.9),
0.95, // 0.9 + ((0.6-0.2)/(1.0-0.2)) * (1.0-0.9) = 0.9 + 0.5 * 0.1 = 0.95
epsilon = EPSILON
);
// Test edge cases
// Hinge at (0, 0)
assert_relative_eq!(interpolate_hinge(0.0, 0.0, 0.0), 0.0, epsilon = EPSILON);
assert_relative_eq!(
interpolate_hinge(0.5, 0.0, 0.0),
0.5, // Should interpolate from (0,0) to (1,1)
epsilon = EPSILON
);
// Hinge at (1, 1)
assert_relative_eq!(
interpolate_hinge(0.5, 1.0, 1.0),
0.5, // Should interpolate from (0,0) to (1,1)
epsilon = EPSILON
);
assert_relative_eq!(interpolate_hinge(1.0, 1.0, 1.0), 1.0, epsilon = EPSILON);
// Test with different numeric types
assert_relative_eq!(
interpolate_hinge(0.5f32, 0.3f32, 0.7f32),
0.7857142857142857, // 0.7 + ((0.5-0.3)/(1.0-0.3)) * (1.0-0.7) = 0.7 + (2/7) * 0.3
epsilon = 1e-7 // Relaxed epsilon for f32 precision
);
assert_relative_eq!(
interpolate_hinge(1u8, 0u8, 1u8), // 1.0, 0.0, 1.0 - should return 1.0
1.0,
epsilon = EPSILON
);
// Test monotonicity - function should be non-decreasing
let test_x = 0.4;
let test_y = 0.6;
let mut prev = interpolate_hinge(0.0, test_x, test_y);
for i in 1..=20 {
let value = i as f64 / 20.0;
let current = interpolate_hinge(value, test_x, test_y);
assert!(
current >= prev,
"Failed monotonicity test at value={}, x={}, y={}, prev={}, current={}",
value,
test_x,
test_y,
prev,
current
);
prev = current;
}
}
#[test]
fn test_interpolate_exponential_graph() {
use textplots::{Chart, Plot, Shape};
println!("\n=== Exponential Interpolation Examples ===");
// Test different steepness values (both positive and negative)
let steepness_values = [-3.0, -1.0, 0.0, 1.0, 3.0];
for steepness in steepness_values.iter() {
println!("\nSteepness = {}", steepness);
// Generate data points
let points: Vec<(f32, f32)> = (0..=50)
.map(|i| {
let x = i as f64 / 50.0;
let y = interpolate_exponential(x, *steepness);
(x as f32, y as f32)
})
.collect();
// Create and display chart
Chart::new(60, 60, 0.0, 1.0)
.lineplot(&Shape::Lines(&points))
.display();
}
}
#[test]
fn test_interpolate_hinge_graph() {
use textplots::{Chart, Plot, Shape};
println!("\n=== hinge Interpolation Examples ===");
// Test different hinge points
let hinges = [(0.3, 0.7), (0.5, 0.5), (0.2, 0.9), (0.8, 0.2)];
for &(x_hinge, y_hinge) in hinges.iter() {
println!("\nHinge at ({}, {})", x_hinge, y_hinge);
// Generate data points
let points: Vec<(f32, f32)> = (0..=50)
.map(|i| {
let x = i as f64 / 50.0;
let y = interpolate_hinge(x, x_hinge, y_hinge);
(x as f32, y as f32)
})
.collect();
// Create and display chart
Chart::new(60, 60, 0.0, 1.0)
.lineplot(&Shape::Lines(&points))
.display();
}
}
#[test]
fn test_interpolate_inflected_exponential_graph() {
use textplots::{Chart, Plot, Shape};
// Test different steepness values
let steepness_values = [0.5, 1.0, 2.0, 5.0];
for steepness in steepness_values.iter() {
println!("\nSteepness = {}", steepness);
// Generate data points
let points: Vec<(f32, f32)> = (0..=50)
.map(|i| {
let x = i as f64 / 50.0;
let y = interpolate_inflected_exponential(x, *steepness);
(x as f32, y as f32)
})
.collect();
// Create and display chart
Chart::new(60, 60, 0.0, 1.0)
.lineplot(&Shape::Lines(&points))
.display();
}
}
#[test]
#[should_panic]
fn test_normalize_invalid_range() {
+7
View File
@@ -9,6 +9,8 @@ export function getUnitIcon(unit: Unit): IconName {
return "lu:Gauge";
case "mm":
return "lu:Ruler";
case "cm":
return "lu:Ruler";
case "m":
return "lu:Ruler";
case "rpm":
@@ -36,6 +38,8 @@ export function renderUnitSymbol(unit: Unit | undefined): string {
return "m/s";
case "mm":
return "mm";
case "cm":
return "cm";
case "m":
return "m";
case "rpm":
@@ -85,6 +89,8 @@ export function renderUnitSymbolLong(unit: Unit): string {
return "meters/second";
case "mm":
return "millimeters";
case "cm":
return "centimeters";
case "m":
return "meters";
case "rpm":
@@ -107,6 +113,7 @@ export function renderUnitSymbolLong(unit: Unit): string {
export const units = [
"m/s",
"mm",
"cm",
"rpm",
"deg",
"m",
@@ -31,6 +31,7 @@ export function Winder2ControlPage() {
tensionArmStateIsLoading,
tensionArmStateIsDisabled,
spoolRpm,
spoolDiameter,
mode,
ExtruderSetMode,
modeIsLoading,
@@ -64,6 +65,12 @@ export function Winder2ControlPage() {
timeseries={spoolRpm}
renderValue={(value) => roundToDecimals(value, 0)}
/>
<TimeSeriesValueNumeric
label="Estimated Diameter"
unit="cm"
timeseries={spoolDiameter}
renderValue={(value) => roundToDecimals(value, 1)}
/>
</ControlCard>
<ControlCard className="bg-red" height={2} title="Traverse">
@@ -15,6 +15,7 @@ import { Unit } from "@/control/units";
export function Winder2GraphsPage() {
const {
spoolRpm,
spoolDiameter,
traversePosition,
traverseState,
tensionArmAngle,
@@ -58,6 +59,13 @@ export function Winder2GraphsPage() {
unit="mm"
renderValue={(value) => roundToDecimals(value, 1)}
/>
<SpoolDiameterGraph
syncHook={syncHook}
newData={spoolDiameter}
unit="cm"
renderValue={(value) => roundToDecimals(value, 1)}
/>
</div>
</div>
@@ -236,3 +244,37 @@ export function PullerSpeedGraph({
/>
);
}
export function SpoolDiameterGraph({
syncHook,
newData,
unit,
renderValue,
}: {
syncHook: ReturnType<typeof useGraphSync>;
newData: TimeSeries | null;
unit?: Unit;
renderValue?: (value: number) => string;
}) {
const config: GraphConfig = {
title: "Estimated Spool Diameter",
icon: "lu:Circle",
colors: {
primary: "#ec4899",
grid: "#e2e8f0",
background: "#ffffff",
},
exportFilename: "spool_diameter_data",
};
return (
<AutoSyncedBigGraph
syncHook={syncHook}
newData={{ newData }}
unit={unit}
renderValue={renderValue}
config={config}
graphId="spool-diameter"
/>
);
}
@@ -7,6 +7,7 @@ import { useWinder2 } from "./useWinder";
import { roundToDecimals } from "@/lib/decimal";
import { Label } from "@/control/Label";
import { SelectionGroupBoolean } from "@/control/SelectionGroup";
import { SelectionGroup } from "@/control/SelectionGroup";
export function Winder2SettingPage() {
const {
@@ -17,7 +18,19 @@ export function Winder2SettingPage() {
pullerSetForward,
pullerStateIsDisabled,
pullerStateIsLoading,
spoolSpeedControllerState,
setRegulationMode,
setMinMaxMinSpeed,
setMinMaxMaxSpeed,
setAdaptiveTensionTarget,
setAdaptiveRadiusLearningRate,
setAdaptiveMaxSpeedMultiplier,
setAdaptiveAccelerationFactor,
setAdaptiveDeaccelerationUrgencyMultiplier,
spoolControllerIsDisabled,
spoolControllerIsLoading,
} = useWinder2();
return (
<Page>
<ControlGrid>
@@ -50,6 +63,145 @@ export function Winder2SettingPage() {
</Label>
</ControlCard>
<ControlCard title="Spool">
<Label label="Speed Algorithm">
<SelectionGroup
value={spoolSpeedControllerState?.data.regulation_mode}
disabled={spoolControllerIsDisabled}
loading={spoolControllerIsLoading}
options={{
MinMax: {
children: "Min/Max",
icon: "lu:ArrowUpDown",
},
Adaptive: {
children: "Adaptive",
icon: "lu:Brain",
},
}}
onChange={(value) =>
setRegulationMode(value as "Adaptive" | "MinMax")
}
/>
</Label>
{spoolSpeedControllerState?.data.regulation_mode === "MinMax" && (
<>
<Label label="Minimum Speed">
<EditValue
value={spoolSpeedControllerState?.data.minmax_min_speed}
title={"Minimum Speed"}
unit="rpm"
step={10}
min={0}
max={600}
defaultValue={50}
renderValue={(value) => roundToDecimals(value, 0)}
onChange={(value) => setMinMaxMinSpeed(value)}
/>
</Label>
<Label label="Maximum Speed">
<EditValue
value={spoolSpeedControllerState?.data.minmax_max_speed}
title={"Maximum Speed"}
unit="rpm"
step={10}
min={0}
max={600}
defaultValue={150}
renderValue={(value) => roundToDecimals(value, 0)}
onChange={(value) => setMinMaxMaxSpeed(value)}
/>
</Label>
</>
)}
{spoolSpeedControllerState?.data.regulation_mode === "Adaptive" && (
<div className="flex flex-row flex-wrap gap-4">
<Label label="Tension Target">
<EditValue
value={
spoolSpeedControllerState?.data.adaptive_tension_target
}
title={"Tension Target"}
unit={undefined}
step={0.01}
min={0}
max={1}
defaultValue={0.7}
renderValue={(value) => roundToDecimals(value, 2)}
onChange={(value) => setAdaptiveTensionTarget(value)}
/>
</Label>
<Label label="Learning Rate">
<EditValue
value={
spoolSpeedControllerState?.data
.adaptive_radius_learning_rate
}
title={"Radius Learning Rate"}
unit={undefined}
step={0.001}
min={0}
max={100}
defaultValue={0.5}
renderValue={(value) => roundToDecimals(value, 2)}
onChange={(value) => setAdaptiveRadiusLearningRate(value)}
/>
</Label>
<Label label="Max Speed Multiplier">
<EditValue
value={
spoolSpeedControllerState?.data
.adaptive_max_speed_multiplier
}
title={"Max Speed Multiplier"}
unit={undefined}
step={0.1}
min={0.1}
max={10}
defaultValue={4.0}
renderValue={(value) => roundToDecimals(value, 1)}
onChange={(value) => setAdaptiveMaxSpeedMultiplier(value)}
/>
</Label>
<Label label="Acceleration Factor">
<EditValue
value={
spoolSpeedControllerState?.data.adaptive_acceleration_factor
}
title={"Acceleration Factor"}
unit={undefined}
step={0.01}
min={0.01}
max={100}
defaultValue={0.2}
renderValue={(value) => roundToDecimals(value, 2)}
onChange={(value) => setAdaptiveAccelerationFactor(value)}
/>
</Label>
<Label label="Deaccel. Urgency">
<EditValue
value={
spoolSpeedControllerState?.data
.adaptive_deacceleration_urgency_multiplier
}
title={"Deacceleration Urgency Multiplier"}
unit={undefined}
step={0.5}
min={1}
max={100}
defaultValue={15.0}
renderValue={(value) => roundToDecimals(value, 1)}
onChange={(value) =>
setAdaptiveDeaccelerationUrgencyMultiplier(value)
}
/>
</Label>
</div>
)}
</ControlCard>
<ControlCard title="Puller">
<Label label="Rotation Direction">
<SelectionGroupBoolean
@@ -10,6 +10,8 @@ import {
PullerStateEvent,
TensionArmStateEvent,
ModeStateEvent,
SpoolRegulationMode,
SpoolSpeedControllerStateEvent,
useWinder2Namespace,
} from "./winder2Namespace";
import { useEffect, useMemo } from "react";
@@ -274,9 +276,11 @@ function useTraverse(
function useSpool(machine_identification_unique: MachineIdentificationUnique) {
// Read Path
const { spoolRpm } = useWinder2Namespace(machine_identification_unique);
const { spoolRpm, spoolDiameter } = useWinder2Namespace(
machine_identification_unique,
);
return { spoolRpm };
return { spoolRpm, spoolDiameter };
}
function usePuller(machine_identification_unique: MachineIdentificationUnique) {
@@ -446,6 +450,234 @@ function useMode(machine_identification_unique: MachineIdentificationUnique): {
};
}
function useSpoolSpeedController(
machine_identification_unique: MachineIdentificationUnique,
): {
spoolSpeedControllerState: SpoolSpeedControllerStateEvent | null;
setRegulationMode: (mode: SpoolRegulationMode) => void;
setMinMaxMinSpeed: (speed: number) => void;
setMinMaxMaxSpeed: (speed: number) => void;
setAdaptiveTensionTarget: (value: number) => void;
setAdaptiveRadiusLearningRate: (value: number) => void;
setAdaptiveMaxSpeedMultiplier: (value: number) => void;
setAdaptiveAccelerationFactor: (value: number) => void;
setAdaptiveDeaccelerationUrgencyMultiplier: (value: number) => void;
spoolControllerIsLoading: boolean;
spoolControllerIsDisabled: boolean;
} {
const spoolStateOptimistic =
useStateOptimistic<SpoolSpeedControllerStateEvent["data"]>();
// Write path - Set regulation mode
const regulationSchema = z.object({
SpoolSetRegulationMode: z.enum(["Adaptive", "MinMax"]),
});
const { request: requestRegulation } = useMachineMutation(regulationSchema);
const setRegulationMode = async (mode: SpoolRegulationMode) => {
if (spoolStateOptimistic.value) {
spoolStateOptimistic.setOptimistic({
...spoolStateOptimistic.value,
regulation_mode: mode,
});
}
requestRegulation({
machine_identification_unique,
data: { SpoolSetRegulationMode: mode },
})
.then((response) => {
if (!response.success) spoolStateOptimistic.resetToReal();
})
.catch(() => spoolStateOptimistic.resetToReal());
};
// Write path - Set minmax min speed
const minSpeedSchema = z.object({
SpoolSetMinMaxMinSpeed: z.number(),
});
const { request: requestMinSpeed } = useMachineMutation(minSpeedSchema);
const setMinMaxMinSpeed = async (speed: number) => {
if (spoolStateOptimistic.value) {
spoolStateOptimistic.setOptimistic({
...spoolStateOptimistic.value,
minmax_min_speed: speed,
});
}
requestMinSpeed({
machine_identification_unique,
data: { SpoolSetMinMaxMinSpeed: speed },
})
.then((response) => {
if (!response.success) spoolStateOptimistic.resetToReal();
})
.catch(() => spoolStateOptimistic.resetToReal());
};
// Write path - Set minmax max speed
const maxSpeedSchema = z.object({
SpoolSetMinMaxMaxSpeed: z.number(),
});
const { request: requestMaxSpeed } = useMachineMutation(maxSpeedSchema);
const setMinMaxMaxSpeed = async (speed: number) => {
if (spoolStateOptimistic.value) {
spoolStateOptimistic.setOptimistic({
...spoolStateOptimistic.value,
minmax_max_speed: speed,
});
}
requestMaxSpeed({
machine_identification_unique,
data: { SpoolSetMinMaxMaxSpeed: speed },
})
.then((response) => {
if (!response.success) spoolStateOptimistic.resetToReal();
})
.catch(() => spoolStateOptimistic.resetToReal());
};
// Write path - Set adaptive parameters
const adaptiveTensionTargetSchema = z.object({
SpoolSetAdaptiveTensionTarget: z.number(),
});
const { request: requestAdaptiveTensionTarget } = useMachineMutation(
adaptiveTensionTargetSchema,
);
const setAdaptiveTensionTarget = async (value: number) => {
if (spoolStateOptimistic.value) {
spoolStateOptimistic.setOptimistic({
...spoolStateOptimistic.value,
adaptive_tension_target: value,
});
}
requestAdaptiveTensionTarget({
machine_identification_unique,
data: { SpoolSetAdaptiveTensionTarget: value },
})
.then((response) => {
if (!response.success) spoolStateOptimistic.resetToReal();
})
.catch(() => spoolStateOptimistic.resetToReal());
};
const adaptiveRadiusLearningRateSchema = z.object({
SpoolSetAdaptiveRadiusLearningRate: z.number(),
});
const { request: requestAdaptiveRadiusLearningRate } = useMachineMutation(
adaptiveRadiusLearningRateSchema,
);
const setAdaptiveRadiusLearningRate = async (value: number) => {
if (spoolStateOptimistic.value) {
spoolStateOptimistic.setOptimistic({
...spoolStateOptimistic.value,
adaptive_radius_learning_rate: value,
});
}
requestAdaptiveRadiusLearningRate({
machine_identification_unique,
data: { SpoolSetAdaptiveRadiusLearningRate: value },
})
.then((response) => {
if (!response.success) spoolStateOptimistic.resetToReal();
})
.catch(() => spoolStateOptimistic.resetToReal());
};
const adaptiveMaxSpeedMultiplierSchema = z.object({
SpoolSetAdaptiveMaxSpeedMultiplier: z.number(),
});
const { request: requestAdaptiveMaxSpeedMultiplier } = useMachineMutation(
adaptiveMaxSpeedMultiplierSchema,
);
const setAdaptiveMaxSpeedMultiplier = async (value: number) => {
if (spoolStateOptimistic.value) {
spoolStateOptimistic.setOptimistic({
...spoolStateOptimistic.value,
adaptive_max_speed_multiplier: value,
});
}
requestAdaptiveMaxSpeedMultiplier({
machine_identification_unique,
data: { SpoolSetAdaptiveMaxSpeedMultiplier: value },
})
.then((response) => {
if (!response.success) spoolStateOptimistic.resetToReal();
})
.catch(() => spoolStateOptimistic.resetToReal());
};
const adaptiveAccelerationFactorSchema = z.object({
SpoolSetAdaptiveAccelerationFactor: z.number(),
});
const { request: requestAdaptiveAccelerationFactor } = useMachineMutation(
adaptiveAccelerationFactorSchema,
);
const setAdaptiveAccelerationFactor = async (value: number) => {
if (spoolStateOptimistic.value) {
spoolStateOptimistic.setOptimistic({
...spoolStateOptimistic.value,
adaptive_acceleration_factor: value,
});
}
requestAdaptiveAccelerationFactor({
machine_identification_unique,
data: { SpoolSetAdaptiveAccelerationFactor: value },
})
.then((response) => {
if (!response.success) spoolStateOptimistic.resetToReal();
})
.catch(() => spoolStateOptimistic.resetToReal());
};
const adaptiveDeaccelerationUrgencyMultiplierSchema = z.object({
SpoolSetAdaptiveDeaccelerationUrgencyMultiplier: z.number(),
});
const { request: requestAdaptiveDeaccelerationUrgencyMultiplier } =
useMachineMutation(adaptiveDeaccelerationUrgencyMultiplierSchema);
const setAdaptiveDeaccelerationUrgencyMultiplier = async (value: number) => {
if (spoolStateOptimistic.value) {
spoolStateOptimistic.setOptimistic({
...spoolStateOptimistic.value,
adaptive_deacceleration_urgency_multiplier: value,
});
}
requestAdaptiveDeaccelerationUrgencyMultiplier({
machine_identification_unique,
data: { SpoolSetAdaptiveDeaccelerationUrgencyMultiplier: value },
})
.then((response) => {
if (!response.success) spoolStateOptimistic.resetToReal();
})
.catch(() => spoolStateOptimistic.resetToReal());
};
// Read path
const { spoolSpeedControllerState } = useWinder2Namespace(
machine_identification_unique,
);
// Update real values from server
useEffect(() => {
if (spoolSpeedControllerState?.data) {
spoolStateOptimistic.setReal(spoolSpeedControllerState.data);
}
}, [spoolSpeedControllerState]);
return {
spoolSpeedControllerState,
setRegulationMode,
setMinMaxMinSpeed,
setMinMaxMaxSpeed,
setAdaptiveTensionTarget,
setAdaptiveRadiusLearningRate,
setAdaptiveMaxSpeedMultiplier,
setAdaptiveAccelerationFactor,
setAdaptiveDeaccelerationUrgencyMultiplier,
spoolControllerIsLoading:
spoolStateOptimistic.isOptimistic || !spoolStateOptimistic.isInitialized,
spoolControllerIsDisabled:
spoolStateOptimistic.isOptimistic || !spoolStateOptimistic.isInitialized,
};
}
export function useWinder2() {
const { serial: serialString } = winder2SerialRoute.useParams();
@@ -480,6 +712,7 @@ export function useWinder2() {
const puller = usePuller(machineIdentification);
const mode = useMode(machineIdentification);
const traverse = useTraverse(machineIdentification);
const spoolSpeedController = useSpoolSpeedController(machineIdentification);
return {
...laserpointerControls,
@@ -488,5 +721,6 @@ export function useWinder2() {
...spool,
...puller,
...traverse,
...spoolSpeedController,
};
}
@@ -117,6 +117,13 @@ export const spoolRpmEventDataSchema = z.object({
rpm: z.number(),
});
/**
* Spool diameter event schema
*/
export const spoolDiameterEventDataSchema = z.object({
diameter: z.number(),
});
export const spoolStateEventDataSchema = z.object({
speed_min: z.number(),
speed_max: z.number(),
@@ -133,6 +140,25 @@ export const tensionArmStateEventDataSchema = z.object({
zeroed: z.boolean(),
});
/**
* Spool speed controller regulation mode enum
*/
export const spoolRegulationModeSchema = z.enum(["Adaptive", "MinMax"]);
/**
* Spool speed controller state event schema
*/
export const spoolSpeedControllerStateEventDataSchema = z.object({
regulation_mode: spoolRegulationModeSchema,
minmax_min_speed: z.number(),
minmax_max_speed: z.number(),
adaptive_tension_target: z.number(),
adaptive_radius_learning_rate: z.number(),
adaptive_max_speed_multiplier: z.number(),
adaptive_acceleration_factor: z.number(),
adaptive_deacceleration_urgency_multiplier: z.number(),
});
// ========== Event Schemas with Wrappers ==========
export const traversePositionEventSchema = eventSchema(
@@ -151,6 +177,9 @@ export const autostopStateEventSchema = eventSchema(
);
export const modeStateEventSchema = eventSchema(modeStateEventDataSchema);
export const spoolRpmEventSchema = eventSchema(spoolRpmEventDataSchema);
export const spoolDiameterEventSchema = eventSchema(
spoolDiameterEventDataSchema,
);
export const spoolStateEventSchema = eventSchema(spoolStateEventDataSchema);
export const tensionArmAngleEventSchema = eventSchema(
tensionArmAngleEventDataSchema,
@@ -158,6 +187,9 @@ export const tensionArmAngleEventSchema = eventSchema(
export const tensionArmStateEventSchema = eventSchema(
tensionArmStateEventDataSchema,
);
export const spoolSpeedControllerStateEventSchema = eventSchema(
spoolSpeedControllerStateEventDataSchema,
);
// ========== Type Inferences ==========
@@ -174,10 +206,15 @@ export type Mode = z.infer<typeof modeSchema>;
export type ModeStateEvent = z.infer<typeof modeStateEventSchema>;
export type SpoolStateEvent = z.infer<typeof spoolStateEventSchema>;
export type MeasurementsWindingRpmEvent = z.infer<typeof spoolRpmEventSchema>;
export type SpoolDiameterEvent = z.infer<typeof spoolDiameterEventSchema>;
export type MeasurementsTensionArmEvent = z.infer<
typeof tensionArmAngleEventSchema
>;
export type TensionArmStateEvent = z.infer<typeof tensionArmStateEventSchema>;
export type SpoolRegulationMode = z.infer<typeof spoolRegulationModeSchema>;
export type SpoolSpeedControllerStateEvent = z.infer<
typeof spoolSpeedControllerStateEventSchema
>;
export type Winder2NamespaceStore = {
// State events (latest only)
@@ -186,12 +223,14 @@ export type Winder2NamespaceStore = {
autostopState: AutostopStateEvent | null;
modeState: ModeStateEvent | null;
tensionArmState: TensionArmStateEvent | null;
spoolSpeedControllerState: SpoolSpeedControllerStateEvent | null;
// Metric events (cached for 1 hour)
traversePosition: TimeSeries;
pullerSpeed: TimeSeries;
autostopWoundedLength: TimeSeries;
spoolRpm: TimeSeries;
spoolDiameter: TimeSeries;
tensionArmAngle: TimeSeries;
};
@@ -214,6 +253,8 @@ const { initialTimeSeries: spoolRpm, insert: addSpoolRpm } = createTimeSeries(
FIVE_SECOND,
ONE_HOUR,
);
const { initialTimeSeries: spoolDiameter, insert: addSpoolDiameter } =
createTimeSeries(TWENTY_MILLISECOND, ONE_SECOND, FIVE_SECOND, ONE_HOUR);
const { initialTimeSeries: tensionArmAngle, insert: addTensionArmAngle } =
createTimeSeries(TWENTY_MILLISECOND, ONE_SECOND, FIVE_SECOND, ONE_HOUR);
@@ -231,12 +272,14 @@ export const createWinder2NamespaceStore =
autostopState: null,
modeState: null,
tensionArmState: null,
spoolSpeedControllerState: null,
// Metric events (cached for 1 hour)
traversePosition,
pullerSpeed,
autostopWoundedLength,
spoolRpm,
spoolDiameter,
tensionArmAngle,
};
});
@@ -297,6 +340,12 @@ export function winder2MessageHandler(
...state,
tensionArmState: event as TensionArmStateEvent,
}));
} else if (eventName === "SpoolSpeedControllerStateEvent") {
console.log("SpoolSpeedControllerStateEvent", event);
updateStore((state) => ({
...state,
spoolSpeedControllerState: event as SpoolSpeedControllerStateEvent,
}));
}
// Metric events (keep for 1 hour)
else if (eventName === "TraversePositionEvent") {
@@ -345,6 +394,16 @@ export function winder2MessageHandler(
...state,
spoolRpm: addSpoolRpm(state.spoolRpm, timeseriesValue),
}));
} else if (eventName === "SpoolDiameterEvent") {
const diameterEvent = event as SpoolDiameterEvent;
const timeseriesValue: TimeSeriesValue = {
value: diameterEvent.data.diameter,
timestamp: event.ts,
};
updateStore((state) => ({
...state,
spoolDiameter: addSpoolDiameter(state.spoolDiameter, timeseriesValue),
}));
} else if (eventName === "TensionArmAngleEvent") {
const angleEvent = event as MeasurementsTensionArmEvent;
const timeseriesValue: TimeSeriesValue = {
+1 -1
View File
@@ -2,7 +2,6 @@ use std::time::{Duration, Instant};
use super::Winder2;
use control_core::actors::Actor;
use tracing::debug;
impl Actor for Winder2 {
fn act(
@@ -40,6 +39,7 @@ impl Actor for Winder2 {
{
self.emit_tension_arm_angle();
self.emit_spool_rpm();
self.emit_spool_diameter();
self.emit_puller_speed();
self.emit_traverse_position();
self.last_measurement_emit = now;
@@ -0,0 +1,459 @@
use crate::machines::winder2::{
clamp_revolution::clamp_revolution_uom, filament_tension::FilamentTensionCalculator,
puller_speed_controller::PullerSpeedController,
};
use super::{clamp_revolution::Clamping, tension_arm::TensionArm};
use control_core::{
controllers::first_degree_motion::angular_acceleration_speed_controller::AngularAccelerationSpeedController,
helpers::{interpolation::scale, moving_time_window::MovingTimeWindow},
};
use core::f64;
use std::time::Instant;
use uom::{
ConstZero,
si::{
angle::degree,
angular_acceleration::radian_per_second_squared,
angular_velocity::{radian_per_second, revolution_per_minute},
f64::{Angle, AngularAcceleration, AngularVelocity, Length},
length::{centimeter, meter},
velocity::meter_per_second,
},
};
/// Adaptive spool speed controller that automatically adjusts to maintain optimal filament tension.
///
/// This controller monitors filament tension via the tension arm and learns the appropriate
/// maximum speed based on puller speed and tension feedback. It uses closed-loop control
/// to minimize tension error and applies smooth acceleration to prevent sudden motor commands.
#[derive(Debug)]
pub struct AdaptiveSpoolSpeedController {
/// Last commanded angular velocity sent to the spool motor
last_speed: AngularVelocity,
/// Whether the speed controller is enabled (false = always returns zero speed)
enabled: bool,
/// Acceleration controller to smooth speed transitions and prevent sudden changes
acceleration_controller: AngularAccelerationSpeedController,
/// Calculator for converting tension arm angle to normalized filament tension
filament_calc: FilamentTensionCalculator,
/// Moving window of recent speeds (in rad/s) used for dynamic acceleration limit calculation
speed_time_window: MovingTimeWindow<f64>,
/// Estimated diameter in cm
radius: Length,
/// Timestamp of last max speed factor update, used for time-aware learning rate calculation
last_max_speed_factor_update: Option<Instant>,
/// Target normalized tension value (0.0-1.0) that the controller tries to maintain
tension_target: f64,
/// Proportional control gain for adaptive learning (negative: higher tension reduces speed)
radius_learning_rate: f64,
/// Speed multiplier when tension is at minimum (max speed factor)
max_speed_multiplier: f64,
/// Base acceleration as a fraction of max possible speed (per second)
acceleration_factor: f64,
/// Urgency multiplier for near-zero target speeds
deacceleration_urgency_multiplier: f64,
}
impl AdaptiveSpoolSpeedController {
/// Maximum speed limit (in RPM) used to initialize the acceleration controller
const INITIAL_MAX_SPEED_RPM: f64 = 150.0;
/// Absolute safety limit (in RPM) that the spool speed can never exceed to protect hardware
const SAFETY_MAX_SPEED_RPM: f64 = 600.0;
/// Time window duration (in seconds) for tracking recent speed history in acceleration calculations
const SPEED_WINDOW_DURATION_SECS: u64 = 5;
/// Maximum number of speed samples stored in the moving time window
const SPEED_WINDOW_MAX_SAMPLES: usize = 10;
/// Maximum angle (in degrees) of the tension arm, representing loosest filament tension
const TENSION_ARM_MAX_ANGLE_DEG: f64 = 90.0;
/// Minimum angle (in degrees) of the tension arm, representing tightest filament tension
const TENSION_ARM_MIN_ANGLE_DEG: f64 = 20.0;
/// Target normalized tension value (0.0-1.0) that the controller tries to maintain
const TENSION_TARGET: f64 = 0.7;
/// Proportional control gain for adaptive learning (negative: higher tension reduces speed)
const RADIUS_LEARNING_RATE: f64 = 0.5;
const RADIUS_MIN: f64 = 4.25;
const RADIUS_MAX: f64 = 20.0;
/// if the tension is the lowest, the speed can be up to 2x the puller speed
const MAX_SPEED_MULTIPLIER: f64 = 4.0;
/// Base acceleration as a fraction of max possible speed (per second)
const ACCELERATION_FACTOR: f64 = 0.2; // 20% of max speed per second
/// Urgency multiplier for near-zero target speeds
const DEACCELERATION_URGENCY_MULTIPLIER: f64 = 15.0;
/// Minimum acceleration limit to prevent completely frozen motion
const MIN_ACCELERATION_LIMIT: f64 = 0.5; // rad/s²
/// Creates a new adaptive spool speed controller with default settings.
///
/// Initializes all internal components and sets baseline parameters.
///
/// # Returns
/// A new `AdaptiveSpoolSpeedController` instance ready for use.
pub fn new() -> Self {
let max_speed = AngularVelocity::new::<revolution_per_minute>(Self::INITIAL_MAX_SPEED_RPM);
Self {
last_speed: AngularVelocity::ZERO,
enabled: false,
acceleration_controller: AngularAccelerationSpeedController::new(
Some(AngularVelocity::ZERO),
Some(max_speed),
-AngularAcceleration::ZERO, // Will be dynamically adjusted
AngularAcceleration::ZERO, // Will be dynamically adjusted
AngularVelocity::ZERO,
),
filament_calc: FilamentTensionCalculator::new(
Angle::new::<degree>(Self::TENSION_ARM_MAX_ANGLE_DEG),
Angle::new::<degree>(Self::TENSION_ARM_MIN_ANGLE_DEG),
),
speed_time_window: MovingTimeWindow::new(
std::time::Duration::from_secs(Self::SPEED_WINDOW_DURATION_SECS),
Self::SPEED_WINDOW_MAX_SAMPLES,
),
radius: Length::new::<centimeter>(4.25),
last_max_speed_factor_update: None,
tension_target: Self::TENSION_TARGET,
radius_learning_rate: Self::RADIUS_LEARNING_RATE,
max_speed_multiplier: Self::MAX_SPEED_MULTIPLIER,
acceleration_factor: Self::ACCELERATION_FACTOR,
deacceleration_urgency_multiplier: Self::DEACCELERATION_URGENCY_MULTIPLIER,
}
}
/// Calculates the current maximum speed based on puller speed and learned factor.
///
/// Uses the fundamental relationship between linear and angular velocity:
/// ω = (v / r) × multiplier
///
/// Where:
/// - ω (omega) = angular velocity in radians per second
/// - v = linear velocity in meters per second
/// - r = radius in meters
/// - multiplier = MAX_SPEED_MULTIPLIER for speed scaling
///
/// This ensures the spool's rim speed is properly scaled relative to the puller's
/// linear speed, with the multiplier allowing for tension control adjustments.
///
/// # Parameters
/// - `puller_speed_controller`: Reference to puller for baseline speed calculation
///
/// # Returns
/// Current maximum angular velocity for the spool in radians per second
fn get_max_speed(&self, puller_speed_controller: &PullerSpeedController) -> AngularVelocity {
AngularVelocity::new::<radian_per_second>(
(puller_speed_controller.last_speed.get::<meter_per_second>()
/ self.radius.get::<meter>())
* self.max_speed_multiplier,
)
}
/// Calculates the desired spool speed based on filament tension feedback.
///
/// Reads the tension arm angle, converts it to normalized tension, and calculates
/// the appropriate speed. Also updates the learned maximum speed based on tension error.
///
/// # Parameters
/// - `t`: Current timestamp for learning updates
/// - `tension_arm`: Reference to tension arm for angle measurements
/// - `puller_speed_controller`: Reference to puller for baseline speed calculations
///
/// # Returns
/// The calculated target angular velocity for the spool motor
fn calculate_speed(
&mut self,
t: Instant,
tension_arm: &TensionArm,
puller_speed_controller: &PullerSpeedController,
) -> AngularVelocity {
let min_speed = AngularVelocity::ZERO;
let max_speed = self.get_max_speed(puller_speed_controller);
// Calculate filament tension from arm angle
let tension_arm_angle = tension_arm.get_angle();
let (clamped_angle, clamping_state) = clamp_revolution_uom(
tension_arm_angle,
self.filament_calc.get_max_angle(), // Inverted because min angle = max tension
self.filament_calc.get_min_angle(),
);
// Return minimum speed if tension arm is at limits
if matches!(clamping_state, Clamping::Min | Clamping::Max) {
return min_speed;
}
// 1.0 means maximum tension (high angle, low speed)
// 0.0 means minimum tension (low angle, high speed)
let filament_tension = self.filament_calc.calc_filament_tension(clamped_angle);
self.update_radius(filament_tension, t);
// Calculate speed based on inverted tension (lower tension = higher speed)
let speed = AngularVelocity::new::<radian_per_second>(scale(
1.0 - filament_tension,
min_speed.get::<radian_per_second>(),
max_speed.get::<radian_per_second>(),
));
speed
}
/// Simplified urgency-weighted acceleration that adapts to current operating conditions.
///
/// This approach uses the current max speed (calculated from puller speed and learned diameter)
/// as the basis for acceleration limits, ensuring smooth operation across all speed ranges
/// while providing instant deceleration capability when approaching zero.
///
/// # Parameters
/// - `target_speed`: Target speed to accelerate towards
/// - `puller_speed_controller`: Reference to puller for current max speed calculation
/// - `t`: Current timestamp for the acceleration controller
///
/// # Returns
/// Smoothly accelerated speed with urgency-weighted limits
fn accelerate_speed(
&mut self,
target_speed: AngularVelocity,
puller_speed_controller: &PullerSpeedController,
t: Instant,
) -> AngularVelocity {
let target_speed_rad_s = target_speed.get::<radian_per_second>();
let current_max_speed = self.get_max_speed(puller_speed_controller);
let max_speed_rad_s = current_max_speed.get::<radian_per_second>();
// Base acceleration proportional to current max operating speed
let base_acceleration = max_speed_rad_s * self.acceleration_factor;
// Simple urgency factor - dramatically increases near zero
let urgency_factor = if target_speed_rad_s.abs() < 0.1 {
self.deacceleration_urgency_multiplier * (1.0 / (target_speed_rad_s.abs() + 0.01))
} else {
1.0
};
// Final acceleration limit
let acceleration_limit =
(base_acceleration * urgency_factor).max(Self::MIN_ACCELERATION_LIMIT);
let acceleration =
AngularAcceleration::new::<radian_per_second_squared>(acceleration_limit);
// Update acceleration controller limits
self.acceleration_controller
.set_max_acceleration(acceleration);
self.acceleration_controller
.set_min_acceleration(-acceleration);
let new_speed = self.acceleration_controller.update(target_speed, t);
// Record for diagnostics
self.speed_time_window
.update(new_speed.get::<radian_per_second>(), t);
new_speed
}
/// Safety function that enforces absolute speed limits.
///
/// Ensures the spool speed never exceeds safe operational limits to protect hardware.
/// Acts as a final safety check after all other speed calculations.
///
/// # Parameters
/// - `speed`: The speed to be safety-checked and clamped
///
/// # Returns
/// Speed guaranteed to be within safe operational limits
fn clamp_speed(&mut self, speed: AngularVelocity) -> AngularVelocity {
let min_speed = AngularVelocity::ZERO;
let max_speed = AngularVelocity::new::<revolution_per_minute>(Self::SAFETY_MAX_SPEED_RPM);
speed.max(min_speed).min(max_speed)
}
/// Adaptive learning algorithm that adjusts maximum speed factor based on tension feedback.
///
/// Uses proportional control to adjust the speed factor based on tension error.
/// The controller learns over time to maintain optimal tension by scaling the
/// baseline speed derived from puller speed.
///
/// # Parameters
/// - `filament_tension`: Normalized tension value
/// - `t`: Current timestamp for time-aware learning rate
fn update_radius(&mut self, filament_tension: f64, t: Instant) {
let delta_t = match self.last_max_speed_factor_update {
Some(last_update) => t.duration_since(last_update).as_secs_f64(),
None => {
// First call, initialize and return early
self.last_max_speed_factor_update = Some(t);
return;
}
};
// positive error means too much tension, so we reduce speed
// negative error means too little tension, so we increase speed
let tension_error = filament_tension - self.tension_target;
// Calculate proportional control adjustment
let proportional_gain = self.radius_learning_rate * delta_t;
let radius_change = tension_error * proportional_gain;
// Update the speed factor directly
let new_radius = (self.radius.get::<centimeter>() + radius_change)
.clamp(Self::RADIUS_MIN, Self::RADIUS_MAX);
self.radius = Length::new::<centimeter>(new_radius); // Convert to cm
self.last_max_speed_factor_update = Some(t);
}
/// Main update function that orchestrates the complete speed control pipeline.
///
/// This is the primary interface that calculates the target speed, applies enable/disable
/// logic, smooths speed changes, and applies safety limits.
///
/// Call this function every control loop iteration to get the commanded spool speed.
///
/// # Parameters
/// - `t`: Current timestamp for time-based calculations
/// - `tension_arm`: Reference to tension arm for feedback
/// - `puller_speed_controller`: Reference for adaptive speed scaling
///
/// # Returns
/// Final commanded angular velocity for the spool motor
pub fn update_speed(
&mut self,
t: Instant,
tension_arm: &TensionArm,
puller_speed_controller: &PullerSpeedController,
) -> AngularVelocity {
let target_speed = self.calculate_speed(t, tension_arm, puller_speed_controller);
let enabled_speed = if self.enabled {
target_speed
} else {
AngularVelocity::ZERO
};
let accelerated_speed = self.accelerate_speed(enabled_speed, puller_speed_controller, t);
// Store speed before clamping to preserve the actual commanded value
self.last_speed = accelerated_speed;
self.clamp_speed(accelerated_speed)
}
/// Enables or disables the speed controller.
///
/// When disabled, the controller returns zero speed regardless of tension feedback.
///
/// # Parameters
/// - `enabled`: True to enable speed control, false to disable
pub fn set_enabled(&mut self, enabled: bool) {
self.enabled = enabled;
}
/// Returns whether the speed controller is currently enabled.
///
/// # Returns
/// True if the controller is enabled, false if disabled
pub fn is_enabled(&self) -> bool {
self.enabled
}
/// Resets the controller to initial state, clearing all learned parameters.
///
/// Use this when starting a new winding operation or after significant
/// process changes that would invalidate learned parameters.
pub fn reset(&mut self) {
self.last_speed = AngularVelocity::ZERO;
let _ = self.acceleration_controller.reset(AngularVelocity::ZERO);
self.radius = Length::new::<centimeter>(4.25);
self.last_max_speed_factor_update = None;
self.tension_target = Self::TENSION_TARGET;
self.radius_learning_rate = Self::RADIUS_LEARNING_RATE;
self.max_speed_multiplier = Self::MAX_SPEED_MULTIPLIER;
self.acceleration_factor = Self::ACCELERATION_FACTOR;
self.deacceleration_urgency_multiplier = Self::DEACCELERATION_URGENCY_MULTIPLIER;
}
/// Returns the last commanded speed from the controller.
///
/// # Returns
/// The angular velocity that was last commanded to the spool motor
pub fn get_speed(&self) -> AngularVelocity {
self.last_speed
}
/// Manually sets the current speed, bypassing normal calculation.
///
/// Use this for manual control or when switching control modes to ensure
/// smooth transitions.
///
/// # Parameters
/// - `speed`: The angular velocity to set as the current speed
pub fn set_speed(&mut self, speed: AngularVelocity) {
self.last_speed = speed;
let _ = self.acceleration_controller.reset(speed);
}
pub fn get_radius(&self) -> Length {
self.radius
}
// Getters and setters for the new configurable parameters
pub fn get_tension_target(&self) -> f64 {
self.tension_target
}
pub fn set_tension_target(&mut self, tension_target: f64) {
self.tension_target = tension_target.clamp(0.0, 1.0);
}
pub fn get_radius_learning_rate(&self) -> f64 {
self.radius_learning_rate
}
pub fn set_radius_learning_rate(&mut self, radius_learning_rate: f64) {
self.radius_learning_rate = radius_learning_rate.max(0.0);
}
pub fn get_max_speed_multiplier(&self) -> f64 {
self.max_speed_multiplier
}
pub fn set_max_speed_multiplier(&mut self, max_speed_multiplier: f64) {
self.max_speed_multiplier = max_speed_multiplier.max(0.1);
}
pub fn get_acceleration_factor(&self) -> f64 {
self.acceleration_factor
}
pub fn set_acceleration_factor(&mut self, acceleration_factor: f64) {
self.acceleration_factor = acceleration_factor.clamp(0.01, 1.0);
}
pub fn get_deacceleration_urgency_multiplier(&self) -> f64 {
self.deacceleration_urgency_multiplier
}
pub fn set_deacceleration_urgency_multiplier(
&mut self,
deacceleration_urgency_multiplier: f64,
) {
self.deacceleration_urgency_multiplier = deacceleration_urgency_multiplier.max(1.0);
}
}
+74
View File
@@ -77,6 +77,18 @@ enum Mutation {
PullerSetTargetDiameter(f64),
PullerSetForward(bool),
// Spool Speed Controller
SpoolSetRegulationMode(super::spool_speed_controller::SpoolSpeedControllerType),
SpoolSetMinMaxMinSpeed(f64),
SpoolSetMinMaxMaxSpeed(f64),
// Adaptive Spool Speed Controller Parameters
SpoolSetAdaptiveTensionTarget(f64),
SpoolSetAdaptiveRadiusLearningRate(f64),
SpoolSetAdaptiveMaxSpeedMultiplier(f64),
SpoolSetAdaptiveAccelerationFactor(f64),
SpoolSetAdaptiveDeaccelerationUrgencyMultiplier(f64),
// Auto Stop
AutostopEnable(bool),
AutostopEnableAlarm(bool),
@@ -228,6 +240,18 @@ impl SpoolRpmEvent {
}
}
#[derive(Serialize, Debug, Clone)]
pub struct SpoolDiameterEvent {
/// diameter in mm
pub diameter: f64,
}
impl SpoolDiameterEvent {
pub fn build(&self) -> Event<Self> {
Event::new("SpoolDiameterEvent", self.clone())
}
}
#[derive(Serialize, Debug, Clone)]
pub struct TensionArmAngleEvent {
/// degree
@@ -252,6 +276,32 @@ impl TensionArmStateEvent {
}
}
#[derive(Serialize, Debug, Clone)]
pub struct SpoolSpeedControllerStateEvent {
/// regulation mode
pub regulation_mode: super::spool_speed_controller::SpoolSpeedControllerType,
/// min speed in rpm for minmax mode
pub minmax_min_speed: f64,
/// max speed in rpm for minmax mode
pub minmax_max_speed: f64,
/// tension target for adaptive mode (0.0-1.0)
pub adaptive_tension_target: f64,
/// radius learning rate for adaptive mode
pub adaptive_radius_learning_rate: f64,
/// max speed multiplier for adaptive mode
pub adaptive_max_speed_multiplier: f64,
/// acceleration factor for adaptive mode
pub adaptive_acceleration_factor: f64,
/// deacceleration urgency multiplier for adaptive mode
pub adaptive_deacceleration_urgency_multiplier: f64,
}
impl SpoolSpeedControllerStateEvent {
pub fn build(&self) -> Event<Self> {
Event::new("SpoolSpeedControllerStateEvent", self.clone())
}
}
pub enum Winder2Events {
TraversePosition(Event<TraversePositionEvent>),
TraverseState(Event<TraverseStateEvent>),
@@ -261,8 +311,10 @@ pub enum Winder2Events {
AutostopState(Event<AutostopStateEvent>),
Mode(Event<ModeStateEvent>),
SpoolRpm(Event<SpoolRpmEvent>),
SpoolDiameter(Event<SpoolDiameterEvent>),
TensionArmAngleEvent(Event<TensionArmAngleEvent>),
TensionArmStateEvent(Event<TensionArmStateEvent>),
SpoolSpeedControllerStateEvent(Event<SpoolSpeedControllerStateEvent>),
}
#[derive(Debug)]
@@ -298,8 +350,10 @@ impl CacheableEvents<Winder2Events> for Winder2Events {
Winder2Events::AutostopState(event) => event.into(),
Winder2Events::Mode(event) => event.into(),
Winder2Events::SpoolRpm(event) => event.into(),
Winder2Events::SpoolDiameter(event) => event.into(),
Winder2Events::TensionArmAngleEvent(event) => event.into(),
Winder2Events::TensionArmStateEvent(event) => event.into(),
Winder2Events::SpoolSpeedControllerStateEvent(event) => event.into(),
}
}
@@ -316,8 +370,10 @@ impl CacheableEvents<Winder2Events> for Winder2Events {
Winder2Events::AutostopState(_) => cache_one,
Winder2Events::Mode(_) => cache_one,
Winder2Events::SpoolRpm(_) => cache_one_hour,
Winder2Events::SpoolDiameter(_) => cache_one_hour,
Winder2Events::TensionArmAngleEvent(_) => cache_one_hour,
Winder2Events::TensionArmStateEvent(_) => cache_one,
Winder2Events::SpoolSpeedControllerStateEvent(_) => cache_one,
}
}
}
@@ -339,6 +395,24 @@ impl MachineApi for Winder2 {
Mutation::PullerSetTargetSpeed(value) => self.puller_set_target_speed(value),
Mutation::PullerSetTargetDiameter(_) => todo!(),
Mutation::PullerSetForward(value) => self.puller_set_forward(value),
Mutation::SpoolSetRegulationMode(mode) => self.spool_set_regulation_mode(mode),
Mutation::SpoolSetMinMaxMinSpeed(speed) => self.spool_set_minmax_min_speed(speed),
Mutation::SpoolSetMinMaxMaxSpeed(speed) => self.spool_set_minmax_max_speed(speed),
Mutation::SpoolSetAdaptiveTensionTarget(value) => {
self.spool_set_adaptive_tension_target(value)
}
Mutation::SpoolSetAdaptiveRadiusLearningRate(value) => {
self.spool_set_adaptive_radius_learning_rate(value)
}
Mutation::SpoolSetAdaptiveMaxSpeedMultiplier(value) => {
self.spool_set_adaptive_max_speed_multiplier(value)
}
Mutation::SpoolSetAdaptiveAccelerationFactor(value) => {
self.spool_set_adaptive_acceleration_factor(value)
}
Mutation::SpoolSetAdaptiveDeaccelerationUrgencyMultiplier(value) => {
self.spool_set_adaptive_deacceleration_urgency_multiplier(value)
}
Mutation::AutostopEnable(_) => todo!(),
Mutation::AutostopEnableAlarm(_) => todo!(),
Mutation::AutostopSetLimit(_) => todo!(),
@@ -0,0 +1,296 @@
use crate::machines::winder2::{
clamp_revolution::clamp_revolution_uom, filament_tension::FilamentTensionCalculator,
puller_speed_controller::PullerSpeedController,
};
use super::{clamp_revolution::Clamping, tension_arm::TensionArm};
use control_core::{
controllers::{
first_degree_motion::angular_acceleration_speed_controller::AngularAccelerationSpeedController,
second_degree_motion::acceleration_position_controller::MotionControllerError,
},
helpers::{
interpolation::{interpolate_exponential, scale},
moving_time_window::MovingTimeWindow,
},
uom_extensions::angular_acceleration::revolution_per_minute_per_second,
};
use std::time::Instant;
use uom::{
ConstZero,
si::{
angle::degree,
angular_acceleration::radian_per_second_squared,
angular_velocity::{radian_per_second, revolution_per_minute, revolution_per_second},
f64::{Angle, AngularAcceleration, AngularVelocity, Length, Velocity},
length::meter,
velocity::meter_per_second,
},
};
#[derive(Debug)]
pub struct MinMaxSpoolSpeedController {
/// Current speed in
last_speed: AngularVelocity,
/// Whether the speed controller is enabled or not
enabled: bool,
/// Acceleration controller to dampen speed change
acceleration_controller: AngularAccelerationSpeedController,
/// Filament tension calculator
filament_calc: FilamentTensionCalculator,
/// Unit is angular velocity in rad/s
speed_time_window: MovingTimeWindow<f64>,
}
impl MinMaxSpoolSpeedController {
/// Parameters:
/// - `min_speed`: Minimum speed
/// - `max_speed`: Maximum speed
/// - `acceleration`: Acceleration
/// - `deceleration`: Deceleration (preferably negative)
pub fn new() -> Self {
let max_speed = AngularVelocity::new::<revolution_per_minute>(150.0);
Self {
last_speed: AngularVelocity::ZERO,
enabled: false,
acceleration_controller: AngularAccelerationSpeedController::new(
Some(AngularVelocity::ZERO),
Some(max_speed),
-AngularAcceleration::ZERO, // Will be dynamically adjusted
AngularAcceleration::ZERO, // Will be dynamically adjusted
AngularVelocity::ZERO,
),
filament_calc: FilamentTensionCalculator::new(
Angle::new::<degree>(90.0),
Angle::new::<degree>(20.0),
),
speed_time_window: MovingTimeWindow::new(
std::time::Duration::from_secs(5),
10, // max samples
),
}
}
}
impl MinMaxSpoolSpeedController {
/// Helper method to get min speed without Option type
fn min_speed(&self) -> AngularVelocity {
self.acceleration_controller
.get_min_speed()
.unwrap_or(AngularVelocity::ZERO)
}
/// Helper method to get max speed without Option type
fn max_speed(&self) -> AngularVelocity {
self.acceleration_controller
.get_max_speed()
.unwrap_or(AngularVelocity::new::<radian_per_second>(f64::INFINITY))
}
/// Calculates the desired speed based on the tension arm angle.
///
/// If the arm is over it's maximum angle, the speed is set to the minimum speed.
/// If the arm is under it's minimum angle, the speed is set to the maximum speed.
/// If the arm is within the range, the speed is interpolated between the minimum and maximum speed based on the tension arm angle.
///
/// Parameters:
/// - `t`: The current time.
/// - `tension_arm`: A reference to the `TensionArm` instance that provides the angle of the tension arm.
///
/// Returns:
/// - speed
fn speed_raw(&mut self, _t: Instant, tension_arm: &TensionArm) -> AngularVelocity {
let min_speed = AngularVelocity::ZERO;
// Convert puller speed to angular velocity using a magic factor
// The factor is adjusted so that the tension arm is reasonably high when a standard spool is at its lowest diameter
// Respect the configured max speed limits by taking the minimum of dynamic calculation and configured max
let max_speed = self.max_speed();
// calculate filament tension
let tension_arm_angle = tension_arm.get_angle();
let tension_arm_revolution = clamp_revolution_uom(
tension_arm_angle,
// inverted because min angle is max tension
self.filament_calc.get_max_angle(),
self.filament_calc.get_min_angle(),
);
match tension_arm_revolution.1 {
Clamping::Min => return min_speed,
Clamping::Max => return min_speed,
_ => {}
};
let filament_tension = self
.filament_calc
.calc_filament_tension(tension_arm_revolution.0);
let filament_tension_inverted = 1.0 - filament_tension;
// use exponetial interpolation to make the speed change more sensitive in the lower range
let filament_tension_exponential = interpolate_exponential(filament_tension_inverted, 2.0);
// interpolate speed linear
let speed = AngularVelocity::new::<radian_per_second>(scale(
filament_tension_exponential,
min_speed.get::<radian_per_second>(),
max_speed.get::<radian_per_second>(),
));
// save speed
return speed;
}
/// Accelerates the speed using the acceleration controller.
///
/// Parameters:
/// - `speed`: The current speed
/// - `t`: The current time.
///
/// Returns:
/// - The new speed after applying acceleration.
fn accelerate_speed(&mut self, speed: AngularVelocity, t: Instant) -> AngularVelocity {
// The min/mac acceleration depends on the max speed of the last 5secs or the target speed (whatever is higher)
let acceleration = AngularAcceleration::new::<radian_per_second_squared>(
self.speed_time_window
.max()
.abs()
.max(speed.get::<radian_per_second>().abs())
// The magic factor is dependent on the scceleration settings on the puller speed controller to reduce oscillation
* 0.5,
);
// Set the acceleration to the controller
self.acceleration_controller
.set_max_acceleration(acceleration);
self.acceleration_controller
.set_min_acceleration(-acceleration);
let new_speed = self.acceleration_controller.update(speed, t);
// add new speed to the time window
self.speed_time_window
.update(new_speed.get::<radian_per_second>(), t);
return new_speed;
}
/// Clamps the speed to the defined minimum and maximum speed.
///
/// Parameters:
/// - `speed`: The speed to be clamped.
///
/// Returns:
/// - The clamped speed, ensuring it is within the range of `min_speed` and `max_speed`.
fn clamp_speed(&mut self, speed: AngularVelocity) -> AngularVelocity {
let min_speed = self.min_speed();
let max_speed = self.max_speed();
if speed < min_speed {
return AngularVelocity::ZERO;
} else if speed > max_speed {
return max_speed;
} else {
return speed;
}
}
}
impl MinMaxSpoolSpeedController {
pub fn update_speed(&mut self, t: Instant, tension_arm: &TensionArm) -> AngularVelocity {
let speed = self.speed_raw(t, tension_arm);
let speed = match self.enabled {
true => speed,
false => AngularVelocity::ZERO,
};
let speed = self.accelerate_speed(speed, t);
// save speed before clamping or it will stay 0.0
self.last_speed = speed;
self.clamp_speed(speed)
}
pub fn set_enabled(&mut self, enabled: bool) {
self.enabled = enabled;
}
pub fn is_enabled(&self) -> bool {
self.enabled
}
pub fn reset(&mut self) {
self.last_speed = AngularVelocity::ZERO;
let _ = self.acceleration_controller.reset(AngularVelocity::ZERO);
}
fn update_acceleration(&mut self) -> Result<(), MotionControllerError> {
// Set acceleration to 1/4 of the range between min and max speed
// The spool will accelerate from min to max speed in 4 seconds
let min_speed = self.min_speed();
let max_speed = self.max_speed();
let range = max_speed - min_speed;
let acceleration = AngularAcceleration::new::<revolution_per_minute_per_second>(
range.get::<revolution_per_minute>() / 4.0,
);
self.acceleration_controller
.set_max_acceleration(acceleration);
self.acceleration_controller
.set_min_acceleration(-acceleration);
Ok(())
}
pub fn set_max_speed(
&mut self,
max_speed: AngularVelocity,
) -> Result<(), MotionControllerError> {
self.acceleration_controller.set_max_speed(Some(max_speed));
self.update_acceleration()?;
Ok(())
}
pub fn set_min_speed(
&mut self,
min_speed: AngularVelocity,
) -> Result<(), MotionControllerError> {
self.acceleration_controller.set_min_speed(Some(min_speed));
self.update_acceleration()?;
Ok(())
}
pub fn get_max_speed(&self) -> AngularVelocity {
self.max_speed()
}
pub fn get_min_speed(&self) -> AngularVelocity {
self.min_speed()
}
pub fn get_speed(&self) -> AngularVelocity {
self.last_speed
}
pub fn set_speed(&mut self, speed: AngularVelocity) {
self.last_speed = speed;
// Also update the acceleration controller's current speed to ensure smooth transitions
let _ = self.acceleration_controller.reset(speed);
}
/// derive the radius from the puller speed and the current angular speed
pub fn get_radius(&self, puller_speed_controller: &PullerSpeedController) -> Length {
let puller_speed: Velocity = puller_speed_controller.get_target_speed();
let angular_speed: AngularVelocity = self.last_speed;
// Calculate the radius using the formula: radius = speed / angular_speed
let radius =
puller_speed.get::<meter_per_second>() / angular_speed.get::<revolution_per_second>();
// Ensure the radius is a normal number, otherwise default to 0.0
let radius = Some(radius).filter(|&n| n.is_normal()).unwrap_or(0.0);
Length::new::<meter>(radius)
}
}
+112 -3
View File
@@ -1,14 +1,16 @@
pub mod act;
pub mod adaptive_spool_speed_controller;
pub mod api;
pub mod clamp_revolution;
pub mod filament_tension;
pub mod minmax_spool_speed_controller;
pub mod new;
pub mod puller_speed_controller;
pub mod spool_speed_controller;
pub mod tension_arm;
pub mod traverse_controller;
use std::{fmt::Debug, time::Instant};
use std::{fmt::Debug, ops::Mul, time::Instant};
use api::{
ModeStateEvent, TensionArmAngleEvent, TensionArmStateEvent, TraversePositionEvent,
@@ -32,7 +34,7 @@ use uom::si::{
angle::degree,
angular_velocity::revolution_per_minute,
f64::{Length, Velocity},
length::millimeter,
length::{centimeter, millimeter},
};
#[derive(Debug)]
@@ -255,6 +257,7 @@ impl Winder2 {
self.emit_mode_state();
self.emit_traverse_state();
self.emit_spool_speed_controller_state();
}
/// Apply the mode changes to the spool
@@ -497,13 +500,23 @@ impl Winder2 {
let event = api::SpoolRpmEvent { rpm }.build();
self.namespace.emit(Winder2Events::SpoolRpm(event))
}
fn emit_spool_diameter(&mut self) {
let diameter = self
.spool_speed_controller
.get_estimated_radius()
.mul(2.0)
.get::<centimeter>();
let event = api::SpoolDiameterEvent { diameter }.build();
self.namespace.emit(Winder2Events::SpoolDiameter(event))
}
}
/// Implement Puller
impl Winder2 {
/// called by `act`
pub fn sync_puller_speed(&mut self, t: Instant) {
let angular_velocity = self.puller_speed_controller.get_angular_velocity(t);
let angular_velocity = self.puller_speed_controller.calc_angular_velocity(t);
let steps_per_second = self
.puller_speed_controller
.converter
@@ -576,6 +589,102 @@ impl Winder2 {
);
self.namespace.emit(event);
}
// Spool Speed Controller API methods
pub fn spool_set_regulation_mode(
&mut self,
regulation_mode: spool_speed_controller::SpoolSpeedControllerType,
) {
self.spool_speed_controller.set_type(regulation_mode);
self.emit_spool_speed_controller_state();
}
/// Set minimum speed for minmax mode in RPM
pub fn spool_set_minmax_min_speed(&mut self, min_speed_rpm: f64) {
let min_speed = uom::si::f64::AngularVelocity::new::<revolution_per_minute>(min_speed_rpm);
if let Err(e) = self.spool_speed_controller.set_minmax_min_speed(min_speed) {
tracing::error!("Failed to set spool min speed: {:?}", e);
}
self.emit_spool_speed_controller_state();
}
/// Set maximum speed for minmax mode in RPM
pub fn spool_set_minmax_max_speed(&mut self, max_speed_rpm: f64) {
let max_speed = uom::si::f64::AngularVelocity::new::<revolution_per_minute>(max_speed_rpm);
if let Err(e) = self.spool_speed_controller.set_minmax_max_speed(max_speed) {
tracing::error!("Failed to set spool max speed: {:?}", e);
}
self.emit_spool_speed_controller_state();
}
/// Set tension target for adaptive mode (0.0-1.0)
pub fn spool_set_adaptive_tension_target(&mut self, tension_target: f64) {
self.spool_speed_controller
.set_adaptive_tension_target(tension_target);
self.emit_spool_speed_controller_state();
}
/// Set radius learning rate for adaptive mode
pub fn spool_set_adaptive_radius_learning_rate(&mut self, radius_learning_rate: f64) {
self.spool_speed_controller
.set_adaptive_radius_learning_rate(radius_learning_rate);
self.emit_spool_speed_controller_state();
}
/// Set max speed multiplier for adaptive mode
pub fn spool_set_adaptive_max_speed_multiplier(&mut self, max_speed_multiplier: f64) {
self.spool_speed_controller
.set_adaptive_max_speed_multiplier(max_speed_multiplier);
self.emit_spool_speed_controller_state();
}
/// Set acceleration factor for adaptive mode
pub fn spool_set_adaptive_acceleration_factor(&mut self, acceleration_factor: f64) {
self.spool_speed_controller
.set_adaptive_acceleration_factor(acceleration_factor);
self.emit_spool_speed_controller_state();
}
/// Set deacceleration urgency multiplier for adaptive mode
pub fn spool_set_adaptive_deacceleration_urgency_multiplier(
&mut self,
deacceleration_urgency_multiplier: f64,
) {
self.spool_speed_controller
.set_adaptive_deacceleration_urgency_multiplier(deacceleration_urgency_multiplier);
self.emit_spool_speed_controller_state();
}
pub fn emit_spool_speed_controller_state(&mut self) {
let event = api::Winder2Events::SpoolSpeedControllerStateEvent(
api::SpoolSpeedControllerStateEvent {
regulation_mode: self.spool_speed_controller.get_type().clone(),
minmax_min_speed: self
.spool_speed_controller
.get_minmax_min_speed()
.get::<revolution_per_minute>(),
minmax_max_speed: self
.spool_speed_controller
.get_minmax_max_speed()
.get::<revolution_per_minute>(),
adaptive_tension_target: self.spool_speed_controller.get_adaptive_tension_target(),
adaptive_radius_learning_rate: self
.spool_speed_controller
.get_adaptive_radius_learning_rate(),
adaptive_max_speed_multiplier: self
.spool_speed_controller
.get_adaptive_max_speed_multiplier(),
adaptive_acceleration_factor: self
.spool_speed_controller
.get_adaptive_acceleration_factor(),
adaptive_deacceleration_urgency_multiplier: self
.spool_speed_controller
.get_adaptive_deacceleration_urgency_multiplier(),
}
.build(),
);
self.namespace.emit(event);
}
}
#[derive(Debug, Clone, PartialEq)]
+4 -4
View File
@@ -1,12 +1,11 @@
use std::time::Instant;
use crate::machines::winder2::puller_speed_controller::PullerSpeedController;
use crate::machines::winder2::traverse_controller::TraverseController;
use super::api::Winder2Namespace;
use super::spool_speed_controller::SpoolSpeedController;
use super::tension_arm::TensionArm;
use super::{Winder2, Winder2Mode};
use crate::machines::winder2::puller_speed_controller::PullerSpeedController;
use crate::machines::winder2::spool_speed_controller::SpoolSpeedController;
use crate::machines::winder2::traverse_controller::TraverseController;
use anyhow::Error;
use control_core::actors::analog_input_getter::AnalogInputGetter;
use control_core::actors::digital_input_getter::DigitalInputGetter;
@@ -388,6 +387,7 @@ impl MachineNewTrait for Winder2 {
new.emit_mode_state();
new.emit_tension_arm_state();
new.emit_puller_state();
new.emit_spool_speed_controller_state();
Ok(new)
})
@@ -102,10 +102,14 @@ impl PullerSpeedController {
self.converter.angular_velocity_to_velocity(angular_speed)
}
pub fn get_angular_velocity(&mut self, t: Instant) -> AngularVelocity {
pub fn calc_angular_velocity(&mut self, t: Instant) -> AngularVelocity {
let speed = self.update_speed(t);
self.speed_to_angular_velocity(speed)
}
pub fn get_target_speed(&self) -> Velocity {
self.target_speed
}
}
#[derive(Serialize, Deserialize, Debug, Clone)]
@@ -1,281 +1,204 @@
use crate::machines::winder2::{
clamp_revolution::clamp_revolution_uom, filament_tension::FilamentTensionCalculator,
adaptive_spool_speed_controller::AdaptiveSpoolSpeedController,
minmax_spool_speed_controller::MinMaxSpoolSpeedController,
puller_speed_controller::PullerSpeedController,
};
use super::{clamp_revolution::Clamping, tension_arm::TensionArm};
use control_core::{
controllers::{
first_degree_motion::angular_acceleration_speed_controller::AngularAccelerationSpeedController,
second_degree_motion::acceleration_position_controller::MotionControllerError,
},
helpers::{
interpolation::{interpolate_exponential, scale},
moving_time_window::MovingTimeWindow,
},
uom_extensions::angular_acceleration::revolution_per_minute_per_second,
controllers::second_degree_motion::acceleration_position_controller::MotionControllerError,
helpers::moving_time_window::MovingTimeWindow,
};
use std::time::Instant;
use tracing::info;
use uom::{
ConstZero,
si::{
angle::degree,
angular_acceleration::radian_per_second_squared,
angular_velocity::{radian_per_second, revolution_per_minute},
f64::{Angle, AngularAcceleration, AngularVelocity},
velocity::meter_per_second,
},
use super::tension_arm::TensionArm;
use serde::{Deserialize, Serialize};
use std::time::{Duration, Instant};
use uom::si::{
f64::{AngularVelocity, Length},
length::meter,
};
#[derive(Debug, Clone, Serialize, Deserialize)]
pub enum SpoolSpeedControllerType {
Adaptive,
MinMax,
}
#[derive(Debug)]
pub struct SpoolSpeedController {
/// Current speed in
last_speed: AngularVelocity,
/// Whether the speed controller is enabled or not
enabled: bool,
/// Linear acceleration controller to dampen speed change
acceleration_controller: AngularAccelerationSpeedController,
/// Filament tension calculator
filament_calc: FilamentTensionCalculator,
/// Unit is angular velocity in rad/s
speed_time_window: MovingTimeWindow<f64>,
adaptive_controller: AdaptiveSpoolSpeedController,
minmax_controller: MinMaxSpoolSpeedController,
r#type: SpoolSpeedControllerType,
radius_history: MovingTimeWindow<f64>,
}
impl SpoolSpeedController {
/// Parameters:
/// - `min_speed`: Minimum speed
/// - `max_speed`: Maximum speed
/// - `acceleration`: Acceleration
/// - `deceleration`: Deceleration (preferably negative)
pub fn new() -> Self {
let max_speed = AngularVelocity::new::<revolution_per_minute>(600.0);
Self {
last_speed: AngularVelocity::ZERO,
enabled: false,
acceleration_controller: AngularAccelerationSpeedController::new(
Some(AngularVelocity::ZERO),
Some(max_speed),
-AngularAcceleration::ZERO, // Will be dynamically adjusted
AngularAcceleration::ZERO, // Will be dynamically adjusted
AngularVelocity::ZERO,
),
filament_calc: FilamentTensionCalculator::new(
Angle::new::<degree>(90.0),
Angle::new::<degree>(20.0),
),
speed_time_window: MovingTimeWindow::new(
std::time::Duration::from_secs(5),
10, // max samples
),
adaptive_controller: AdaptiveSpoolSpeedController::new(),
minmax_controller: MinMaxSpoolSpeedController::new(),
radius_history: MovingTimeWindow::new(Duration::from_secs(10), 40), // Example size, adjust as needed
r#type: SpoolSpeedControllerType::MinMax,
}
}
}
impl SpoolSpeedController {
/// Helper method to get min speed without Option type
fn min_speed(&self) -> AngularVelocity {
self.acceleration_controller
.get_min_speed()
.unwrap_or(AngularVelocity::ZERO)
pub fn get_speed(&self) -> AngularVelocity {
match self.r#type {
SpoolSpeedControllerType::Adaptive => self.adaptive_controller.get_speed(),
SpoolSpeedControllerType::MinMax => self.minmax_controller.get_speed(),
}
}
/// Helper method to get max speed without Option type
fn max_speed(&self) -> AngularVelocity {
self.acceleration_controller
.get_max_speed()
.unwrap_or(AngularVelocity::new::<radian_per_second>(f64::INFINITY))
pub fn set_speed(&mut self, speed: AngularVelocity) {
match self.r#type {
SpoolSpeedControllerType::Adaptive => self.adaptive_controller.set_speed(speed),
SpoolSpeedControllerType::MinMax => self.minmax_controller.set_speed(speed),
}
}
/// Calculates the desired speed based on the tension arm angle.
///
/// If the arm is over it's maximum angle, the speed is set to the minimum speed.
/// If the arm is under it's minimum angle, the speed is set to the maximum speed.
/// If the arm is within the range, the speed is interpolated between the minimum and maximum speed based on the tension arm angle.
///
/// Parameters:
/// - `t`: The current time.
/// - `tension_arm`: A reference to the `TensionArm` instance that provides the angle of the tension arm.
///
/// Returns:
/// - speed
fn speed_raw(
pub fn set_enabled(&mut self, enabled: bool) {
self.adaptive_controller.set_enabled(enabled);
self.minmax_controller.set_enabled(enabled);
}
pub fn is_enabled(&self) -> bool {
match self.r#type {
SpoolSpeedControllerType::Adaptive => self.adaptive_controller.is_enabled(),
SpoolSpeedControllerType::MinMax => self.minmax_controller.is_enabled(),
}
}
pub fn set_type(&mut self, r#type: SpoolSpeedControllerType) {
// If we're switching to a different type, copy the current speed to the target controller
if std::mem::discriminant(&self.r#type) != std::mem::discriminant(&r#type) {
// Get the current speed from the active controller
let current_speed = match self.r#type {
SpoolSpeedControllerType::Adaptive => self.adaptive_controller.get_speed(),
SpoolSpeedControllerType::MinMax => self.minmax_controller.get_speed(),
};
// Set the speed in the target controller and reset it for smooth transition
match r#type {
SpoolSpeedControllerType::Adaptive => {
self.adaptive_controller.set_speed(current_speed);
self.adaptive_controller.reset();
self.adaptive_controller.set_speed(current_speed); // Set again after reset to maintain speed
}
SpoolSpeedControllerType::MinMax => {
self.minmax_controller.set_speed(current_speed);
self.minmax_controller.reset();
self.minmax_controller.set_speed(current_speed); // Set again after reset to maintain speed
}
}
}
self.r#type = r#type;
}
pub fn get_type(&self) -> &SpoolSpeedControllerType {
&self.r#type
}
pub fn set_minmax_min_speed(
&mut self,
_t: Instant,
tension_arm: &TensionArm,
puller_speed_controller: &PullerSpeedController,
) -> AngularVelocity {
let min_speed = AngularVelocity::ZERO;
// Convert puller speed to angular velocity using a magic factor
// The factor is adjusted so that the tension arm is reasonably high when a standard spool is at its lowest diameter
let max_speed = AngularVelocity::new::<radian_per_second>(
puller_speed_controller.last_speed.get::<meter_per_second>() * 750.0,
);
// calculate filament tension
let tension_arm_angle = tension_arm.get_angle();
let tension_arm_revolution = clamp_revolution_uom(
tension_arm_angle,
// inverted because min angle is max tension
self.filament_calc.get_max_angle(),
self.filament_calc.get_min_angle(),
);
match tension_arm_revolution.1 {
Clamping::Min => return min_speed,
Clamping::Max => return min_speed,
_ => {}
};
let filament_tension = self
.filament_calc
.calc_filament_tension(tension_arm_revolution.0);
let filament_tension_inverted = 1.0 - filament_tension;
// use exponetial interpolation to make the speed change more sensitive in the lower range
let filament_tension_exponential = interpolate_exponential(filament_tension_inverted, 2.0);
// interpolate speed linear
let speed = AngularVelocity::new::<radian_per_second>(scale(
filament_tension_exponential,
min_speed.get::<radian_per_second>(),
max_speed.get::<radian_per_second>(),
));
// save speed
return speed;
min_speed: AngularVelocity,
) -> Result<(), MotionControllerError> {
self.minmax_controller.set_min_speed(min_speed)
}
/// Accelerates the speed using the acceleration controller.
///
/// Parameters:
/// - `speed`: The current speed
/// - `t`: The current time.
///
/// Returns:
/// - The new speed after applying acceleration.
fn accelerate_speed(&mut self, speed: AngularVelocity, t: Instant) -> AngularVelocity {
// The min/mac acceleration depends on the max speed of the last 5secs or the target speed (whatever is higher)
let acceleration = AngularAcceleration::new::<radian_per_second_squared>(
self.speed_time_window
.max()
.abs()
.max(speed.get::<radian_per_second>().abs())
// The magic factor is dependent on the scceleration settings on the puller speed controller to reduce oscillation
* 0.5,
);
// Set the acceleration to the controller
self.acceleration_controller
.set_max_acceleration(acceleration);
self.acceleration_controller
.set_min_acceleration(-acceleration);
let new_speed = self.acceleration_controller.update(speed, t);
// add new speed to the time window
self.speed_time_window
.update(new_speed.get::<radian_per_second>(), t);
return new_speed;
pub fn set_minmax_max_speed(
&mut self,
max_speed: AngularVelocity,
) -> Result<(), MotionControllerError> {
self.minmax_controller.set_max_speed(max_speed)
}
/// Clamps the speed to the defined minimum and maximum speed.
///
/// Parameters:
/// - `speed`: The speed to be clamped.
///
/// Returns:
/// - The clamped speed, ensuring it is within the range of `min_speed` and `max_speed`.
fn clamp_speed(&mut self, speed: AngularVelocity) -> AngularVelocity {
let min_speed = self.min_speed();
let max_speed = self.max_speed();
pub fn get_minmax_min_speed(&self) -> AngularVelocity {
self.minmax_controller.get_min_speed()
}
if speed < min_speed {
return AngularVelocity::ZERO;
} else if speed > max_speed {
return max_speed;
} else {
return speed;
}
pub fn get_minmax_max_speed(&self) -> AngularVelocity {
self.minmax_controller.get_max_speed()
}
}
impl SpoolSpeedController {
pub fn update_speed(
&mut self,
t: Instant,
tension_arm: &TensionArm,
puller_speed_controller: &PullerSpeedController,
) -> AngularVelocity {
let speed = self.speed_raw(t, tension_arm, puller_speed_controller);
let speed = match self.enabled {
true => speed,
false => AngularVelocity::ZERO,
let angular_velocity = match self.r#type {
SpoolSpeedControllerType::Adaptive => {
self.adaptive_controller
.update_speed(t, tension_arm, puller_speed_controller)
}
SpoolSpeedControllerType::MinMax => self.minmax_controller.update_speed(t, tension_arm),
};
let speed = self.accelerate_speed(speed, t);
// save speed before clamping or it will stay 0.0
self.last_speed = speed;
{
let radius = match self.r#type {
SpoolSpeedControllerType::Adaptive => self.adaptive_controller.get_radius(),
SpoolSpeedControllerType::MinMax => {
self.minmax_controller.get_radius(puller_speed_controller)
}
};
self.clamp_speed(speed)
// add radius to history
self.radius_history.update(radius.get::<meter>(), t);
}
angular_velocity
}
pub fn set_enabled(&mut self, enabled: bool) {
self.enabled = enabled;
pub fn get_estimated_radius(&mut self) -> Length {
// Use the average of the last 10 radius measurements
Length::new::<meter>(self.radius_history.average())
}
pub fn reset(&mut self) {
self.last_speed = AngularVelocity::ZERO;
let _ = self.acceleration_controller.reset(AngularVelocity::ZERO);
// Adaptive controller parameter getters and setters
pub fn get_adaptive_tension_target(&self) -> f64 {
self.adaptive_controller.get_tension_target()
}
fn update_acceleration(&mut self) -> Result<(), MotionControllerError> {
// Set acceleration to 1/4 of the range between min and max speed
// The spool will accelerate from min to max speed in 4 seconds
let min_speed = self.min_speed();
let max_speed = self.max_speed();
let range = max_speed - min_speed;
let acceleration = AngularAcceleration::new::<revolution_per_minute_per_second>(
range.get::<revolution_per_minute>() / 4.0,
);
self.acceleration_controller
.set_max_acceleration(acceleration);
self.acceleration_controller
.set_min_acceleration(-acceleration);
Ok(())
pub fn set_adaptive_tension_target(&mut self, tension_target: f64) {
self.adaptive_controller.set_tension_target(tension_target);
}
pub fn set_max_speed(
pub fn get_adaptive_radius_learning_rate(&self) -> f64 {
self.adaptive_controller.get_radius_learning_rate()
}
pub fn set_adaptive_radius_learning_rate(&mut self, radius_learning_rate: f64) {
self.adaptive_controller
.set_radius_learning_rate(radius_learning_rate);
}
pub fn get_adaptive_max_speed_multiplier(&self) -> f64 {
self.adaptive_controller.get_max_speed_multiplier()
}
pub fn set_adaptive_max_speed_multiplier(&mut self, max_speed_multiplier: f64) {
self.adaptive_controller
.set_max_speed_multiplier(max_speed_multiplier);
}
pub fn get_adaptive_acceleration_factor(&self) -> f64 {
self.adaptive_controller.get_acceleration_factor()
}
pub fn set_adaptive_acceleration_factor(&mut self, acceleration_factor: f64) {
self.adaptive_controller
.set_acceleration_factor(acceleration_factor);
}
pub fn get_adaptive_deacceleration_urgency_multiplier(&self) -> f64 {
self.adaptive_controller
.get_deacceleration_urgency_multiplier()
}
pub fn set_adaptive_deacceleration_urgency_multiplier(
&mut self,
max_speed: AngularVelocity,
) -> Result<(), MotionControllerError> {
self.acceleration_controller.set_max_speed(Some(max_speed));
self.update_acceleration()?;
Ok(())
}
pub fn set_min_speed(
&mut self,
min_speed: AngularVelocity,
) -> Result<(), MotionControllerError> {
self.acceleration_controller.set_min_speed(Some(min_speed));
self.update_acceleration()?;
Ok(())
}
pub fn get_max_speed(&self) -> AngularVelocity {
self.max_speed()
}
pub fn get_min_speed(&self) -> AngularVelocity {
self.min_speed()
}
pub fn get_speed(&self) -> AngularVelocity {
self.last_speed
deacceleration_urgency_multiplier: f64,
) {
self.adaptive_controller
.set_deacceleration_urgency_multiplier(deacceleration_urgency_multiplier);
}
}
@@ -6,7 +6,6 @@ use control_core::{
},
converters::linear_step_converter::LinearStepConverter,
};
use tracing::info;
use uom::{
ConstZero,
si::{