dsmc 0.3.0

Digitalservo Motion Control Library
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186

//! Arithmetic on `TransferFunction`: `G1 * G2`, `G1 / G2`, `G1 + G2`, constant gains (`G * k`, `G / k`, ...)
//! and constant offsets (`G + k`).

use super::TransferFunction;
use crate::Polynomial;
use num_traits::Float;
use std::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign};

// ---- Transfer function × transfer function -------------------------------------------------

impl<T: Float + AddAssign, D> Mul for &TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn mul(self, rhs: &TransferFunction<T, D>) -> TransferFunction<T, D> {
        TransferFunction::from_polynomials(
            &self.numerator * &rhs.numerator,
            &self.denominator * &rhs.denominator,
        )
        .reduced()
    }
}

impl<T: Float + AddAssign, D> Mul for TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn mul(self, rhs: TransferFunction<T, D>) -> TransferFunction<T, D> {
        &self * &rhs
    }
}

impl<T: Float + AddAssign, D> Div for &TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn div(self, rhs: &TransferFunction<T, D>) -> TransferFunction<T, D> {
        // (n1/d1) / (n2/d2) = (n1/d1) * (d2/n2)
        let rhs_inv = TransferFunction::from_polynomials(rhs.denominator.clone(), rhs.numerator.clone());
        self * &rhs_inv
    }
}

impl<T: Float + AddAssign, D> Div for TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn div(self, rhs: TransferFunction<T, D>) -> TransferFunction<T, D> {
        &self / &rhs
    }
}

impl<T: Float + AddAssign, D> Add for &TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn add(self, rhs: &TransferFunction<T, D>) -> TransferFunction<T, D> {
        // n1/d1 + n2/d2 = (n1*d2 + n2*d1) / (d1*d2); `reduced()` then cancels any factor
        // shared by d1 and d2 (and any other common numerator/denominator roots), which is
        // equivalent to reducing to a common denominator first.
        let n1d2 = &self.numerator * &rhs.denominator;
        let n2d1 = &self.denominator * &rhs.numerator;
        let numerator = &n1d2 + &n2d1;
        let denominator = &self.denominator * &rhs.denominator;
        TransferFunction::from_polynomials(numerator, denominator).reduced()
    }
}

impl<T: Float + AddAssign, D> Add for TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn add(self, rhs: TransferFunction<T, D>) -> TransferFunction<T, D> {
        &self + &rhs
    }
}

// ---- Constant gain -------------------------------------------------------------------------
// Only the numerator is scaled. Poles/zeros are unchanged, so no `reduced()`.
// `*=` / `/=` do the work; `*` / `/` are built on them. (`&G` is copied with `from_polynomials`
// rather than `clone()`, since the derived `Clone` would require `D: Clone`.)

impl<T: Float, D> MulAssign<T> for TransferFunction<T, D> {
    fn mul_assign(&mut self, k: T) {
        self.numerator.iter_mut().for_each(|c| *c = *c * k);
    }
}

impl<T: Float, D> DivAssign<T> for TransferFunction<T, D> {
    fn div_assign(&mut self, k: T) {
        self.numerator.iter_mut().for_each(|c| *c = *c / k);
    }
}

impl<T: Float, D> Mul<T> for TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn mul(mut self, k: T) -> TransferFunction<T, D> {
        self *= k;
        self
    }
}

impl<T: Float, D> Mul<T> for &TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn mul(self, k: T) -> TransferFunction<T, D> {
        TransferFunction::from_polynomials(self.numerator.clone(), self.denominator.clone()) * k
    }
}

impl<T: Float, D> Div<T> for TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn div(mut self, k: T) -> TransferFunction<T, D> {
        self /= k;
        self
    }
}

impl<T: Float, D> Div<T> for &TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn div(self, k: T) -> TransferFunction<T, D> {
        TransferFunction::from_polynomials(self.numerator.clone(), self.denominator.clone()) / k
    }
}

// ---- Feedback ------------------------------------------------------------------------------

impl<T: Float + AddAssign, D> TransferFunction<T, D> {
    /// Closed-loop transfer function of `self` (open loop `L`) under unity negative feedback,
    /// `L / (1 + L)`. Computed directly as `n / (d + n)`; `n + d` shares a root with `n` only if
    /// `d` does, so no `reduced()` is needed when `self` is already reduced.
    pub fn unity_feedback(&self) -> Self {
        TransferFunction::from_polynomials(self.numerator.clone(), &self.denominator + &self.numerator)
    }
}

// ---- Constant offset -----------------------------------------------------------------------
// n/d + k = (n + k*d) / d. Poles are unchanged, and n + k*d shares a root with d only if n does,
// so no `reduced()` is needed.

impl<T: Float + AddAssign, D> AddAssign<T> for TransferFunction<T, D> {
    fn add_assign(&mut self, k: T) {
        let kd = Polynomial(self.denominator.iter().map(|&c| c * k).collect());
        self.numerator += &kd;
    }
}

impl<T: Float + AddAssign, D> Add<T> for TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn add(mut self, k: T) -> TransferFunction<T, D> {
        self += k;
        self
    }
}

impl<T: Float + AddAssign, D> Add<T> for &TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn add(self, k: T) -> TransferFunction<T, D> {
        TransferFunction::from_polynomials(self.numerator.clone(), self.denominator.clone()) + k
    }
}

// `k * G` / `k + G`: the orphan rule forbids `impl<T> Mul<TransferFunction<T, D>> for T`, so the
// left-hand scalar is implemented per concrete float type. Only f64: with both f32 and f64,
// `10.0 * &g` fails to infer whenever `g`'s `T` is still an unresolved `{float}` (e.g. built by
// `tf!` or from literal slices). For f32, use `g * k` / `g + k`.
macro_rules! impl_scalar_lhs_ops {
    ($($t:ty),*) => {$(
        impl<D> Mul<TransferFunction<$t, D>> for $t {
            type Output = TransferFunction<$t, D>;
            fn mul(self, tf: TransferFunction<$t, D>) -> TransferFunction<$t, D> {
                tf * self
            }
        }

        impl<D> Mul<&TransferFunction<$t, D>> for $t {
            type Output = TransferFunction<$t, D>;
            fn mul(self, tf: &TransferFunction<$t, D>) -> TransferFunction<$t, D> {
                tf * self
            }
        }

        impl<D> Add<TransferFunction<$t, D>> for $t {
            type Output = TransferFunction<$t, D>;
            fn add(self, tf: TransferFunction<$t, D>) -> TransferFunction<$t, D> {
                tf + self
            }
        }

        impl<D> Add<&TransferFunction<$t, D>> for $t {
            type Output = TransferFunction<$t, D>;
            fn add(self, tf: &TransferFunction<$t, D>) -> TransferFunction<$t, D> {
                tf + self
            }
        }
    )*};
}

impl_scalar_lhs_ops!(f64);