vexide_math/feedforward.rs
1//! Simple feedforward controller for motors.
2//! Computes the voltage to maintain an idealized DC motor in a certain state.
3//! Uses this feedforward model: V = Kₛ sign(ω) + Kᵥ ω + Kₐ α
4
5/// Feedforward controller for motor control.
6///
7/// This controller is used to apply feedforward control to achieve desired motor behavior
8/// based on velocity and acceleration.
9#[derive(Debug, Clone)]
10pub struct MotorFeedforwardController {
11 /// Feedforward constant for static friction compensation.
12 pub ks: f32,
13 /// Feedforward constant for velocity compensation.
14 pub kv: f32,
15 /// Feedforward constant for acceleration compensation.
16 pub ka: f32,
17 /// Feedforward constant for the target acceleration.
18 pub target_acceleration: f32,
19 /// Target.
20 pub target: f32,
21}
22
23impl MotorFeedforwardController {
24 /// Creates a new [`MotorFeedforwardController`] with the given constants and target.
25 ///
26 /// # Arguments
27 ///
28 /// * `ks` - Feedforward constant for static friction compensation.
29 /// * `kv` - Feedforward constant for velocity compensation.
30 /// * `ka` - Feedforward constant for acceleration compensation.
31 /// * `target_acceleration` - Feedforward constant for the target acceleration.
32 ///
33 /// # Returns
34 ///
35 /// A new [`MotorFeedforwardController`].
36 pub const fn new(ks: f32, kv: f32, ka: f32, target_acceleration: f32) -> Self {
37 Self {
38 ks,
39 kv,
40 ka,
41 target_acceleration,
42 target: 0.0,
43 }
44 }
45
46 /// Calculates the control output.
47 ///
48 /// # Arguments
49 ///
50 /// * `target_acceleration` - The target_acceleration of the system.
51 /// * `target` - Target.
52 ///
53 /// # Returns
54 ///
55 /// The control output to apply to the motor.
56 pub fn calculate(&self, target: f32, target_acceleration: f32) -> f32 {
57 // Calculate the feedforward component based on velocity and acceleration
58 self.ks * num::signum(target) + self.kv * target + self.ka * target_acceleration
59 }
60}