@@ -37,7 +37,10 @@ pub struct ControlOutput<T: Float> {
37
37
pub output : T ,
38
38
}
39
39
40
- impl < T > Pid < T > where T : Float {
40
+ impl < T > Pid < T >
41
+ where
42
+ T : Float ,
43
+ {
41
44
pub fn new ( kp : T , ki : T , kd : T , p_limit : T , i_limit : T , d_limit : T ) -> Self {
42
45
Self {
43
46
kp,
@@ -86,17 +89,14 @@ impl<T> Pid<T> where T: Float {
86
89
self . integral_term = self . integral_term + error * self . ki ;
87
90
// Mitigate integral windup: Don't want to keep building up error
88
91
// beyond what i_limit will allow.
89
- self . integral_term = self . i_limit . min ( self . integral_term . abs ( ) ) * self . integral_term . signum ( ) ;
92
+ self . integral_term =
93
+ self . i_limit . min ( self . integral_term . abs ( ) ) * self . integral_term . signum ( ) ;
90
94
91
95
// Mitigate derivative kick: Use the derivative of the measurement
92
96
// rather than the derivative of the error.
93
97
let d_unbounded = -match self . prev_measurement . as_ref ( ) {
94
- Some ( prev_measurement) => {
95
- measurement - * prev_measurement
96
- } ,
97
- None => {
98
- T :: zero ( )
99
- }
98
+ Some ( prev_measurement) => measurement - * prev_measurement,
99
+ None => T :: zero ( ) ,
100
100
} * self . kd ;
101
101
self . prev_measurement = Some ( measurement) ;
102
102
let d = self . d_limit . min ( d_unbounded. abs ( ) ) * d_unbounded. signum ( ) ;
@@ -105,12 +105,11 @@ impl<T> Pid<T> where T: Float {
105
105
p,
106
106
i : self . integral_term ,
107
107
d,
108
- output : ( p + self . integral_term + d)
108
+ output : ( p + self . integral_term + d) ,
109
109
}
110
110
}
111
111
}
112
112
113
-
114
113
#[ cfg( test) ]
115
114
mod tests {
116
115
use super :: Pid ;
@@ -178,27 +177,27 @@ mod tests {
178
177
pid. update_setpoint ( 10.0 ) ;
179
178
180
179
let out = pid. next_control_output ( 0.0 ) ;
181
- assert_eq ! ( out. p, 10.0 ) ; // 1.0 * 10.0
182
- assert_eq ! ( out. i, 1.0 ) ; // 0.1 * 10.0
183
- assert_eq ! ( out. d, 0.0 ) ; // -(1.0 * 0.0)
180
+ assert_eq ! ( out. p, 10.0 ) ; // 1.0 * 10.0
181
+ assert_eq ! ( out. i, 1.0 ) ; // 0.1 * 10.0
182
+ assert_eq ! ( out. d, 0.0 ) ; // -(1.0 * 0.0)
184
183
assert_eq ! ( out. output, 11.0 ) ;
185
184
186
185
let out = pid. next_control_output ( 5.0 ) ;
187
- assert_eq ! ( out. p, 5.0 ) ; // 1.0 * 5.0
188
- assert_eq ! ( out. i, 1.5 ) ; // 0.1 * (10.0 + 5.0)
189
- assert_eq ! ( out. d, -5.0 ) ; // -(1.0 * 5.0)
186
+ assert_eq ! ( out. p, 5.0 ) ; // 1.0 * 5.0
187
+ assert_eq ! ( out. i, 1.5 ) ; // 0.1 * (10.0 + 5.0)
188
+ assert_eq ! ( out. d, -5.0 ) ; // -(1.0 * 5.0)
190
189
assert_eq ! ( out. output, 1.5 ) ;
191
190
192
191
let out = pid. next_control_output ( 11.0 ) ;
193
- assert_eq ! ( out. p, -1.0 ) ; // 1.0 * -1.0
194
- assert_eq ! ( out. i, 1.4 ) ; // 0.1 * (10.0 + 5.0 - 1)
195
- assert_eq ! ( out. d, -6.0 ) ; // -(1.0 * 6.0)
192
+ assert_eq ! ( out. p, -1.0 ) ; // 1.0 * -1.0
193
+ assert_eq ! ( out. i, 1.4 ) ; // 0.1 * (10.0 + 5.0 - 1)
194
+ assert_eq ! ( out. d, -6.0 ) ; // -(1.0 * 6.0)
196
195
assert_eq ! ( out. output, -5.6 ) ;
197
196
198
197
let out = pid. next_control_output ( 10.0 ) ;
199
- assert_eq ! ( out. p, 0.0 ) ; // 1.0 * 0.0
200
- assert_eq ! ( out. i, 1.4 ) ; // 0.1 * (10.0 + 5.0 - 1.0 + 0.0)
201
- assert_eq ! ( out. d, 1.0 ) ; // -(1.0 * -1.0)
198
+ assert_eq ! ( out. p, 0.0 ) ; // 1.0 * 0.0
199
+ assert_eq ! ( out. i, 1.4 ) ; // 0.1 * (10.0 + 5.0 - 1.0 + 0.0)
200
+ assert_eq ! ( out. d, 1.0 ) ; // -(1.0 * -1.0)
202
201
assert_eq ! ( out. output, 2.4 ) ;
203
202
}
204
203
@@ -210,8 +209,14 @@ mod tests {
210
209
let mut pid64 = Pid :: new ( 2.0f64 , 0.0 , 0.0 , 100.0 , 100.0 , 100.0 ) ;
211
210
pid64. update_setpoint ( 10.0 ) ;
212
211
213
- assert_eq ! ( pid32. next_control_output( 0.0 ) . output, pid64. next_control_output( 0.0 ) . output as f32 ) ;
214
- assert_eq ! ( pid32. next_control_output( 0.0 ) . output as f64 , pid64. next_control_output( 0.0 ) . output) ;
212
+ assert_eq ! (
213
+ pid32. next_control_output( 0.0 ) . output,
214
+ pid64. next_control_output( 0.0 ) . output as f32
215
+ ) ;
216
+ assert_eq ! (
217
+ pid32. next_control_output( 0.0 ) . output as f64 ,
218
+ pid64. next_control_output( 0.0 ) . output
219
+ ) ;
215
220
}
216
221
217
222
}
0 commit comments