Compare commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
69a3cdbef3 | ||
|
|
ff51018d35 | ||
|
|
a228906eed | ||
|
|
a238fc9246 | ||
|
|
82a7e2fbfa | ||
|
|
1d78bacbe8 | ||
|
|
043e781049 | ||
|
|
2d23d9db2f |
No files matched your search
Generated
+1
@@ -592,6 +592,7 @@ dependencies = [
|
||||
"serialport",
|
||||
"smol",
|
||||
"socketioxide",
|
||||
"textplots",
|
||||
"tracing",
|
||||
"uom",
|
||||
]
|
||||
|
||||
@@ -22,3 +22,4 @@ tracing = "0.1.41"
|
||||
erased-serde = "0.4.6"
|
||||
[dev-dependencies]
|
||||
approx = "0.5.1"
|
||||
textplots = "0.8"
|
||||
@@ -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() {
|
||||
|
||||
@@ -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 = {
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
}
|
||||
}
|
||||
@@ -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)]
|
||||
|
||||
@@ -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::{
|
||||
|
||||
Reference in new issue
Block a user