kernelvex 0.2.2

VEX Controls 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

use crate::util::si::{QLength, QTime};
use libm::{sqrt};

/// A single time-indexed state during motion.
/// Velocity and acceleration are stored as f64, as their dimensional analysis
/// is handled implicitly by the profile generation logic.
#[derive(Debug, Clone, Copy)]
pub struct MotionState {
    pub time: QTime,
    /// Position (distance) from start.
    pub position: QLength,
    /// Velocity at this time point (m/s).
    pub velocity: f64,
    /// Acceleration at this time point (m/s^2).
    pub acceleration: f64,
}
/// Constraints for defining the motion envelope.
#[derive(Debug, Clone, Copy)]
pub struct TrapezoidalConstraints {
    /// Maximum achievable velocity (m/s).
    pub max_velocity: f64,
    /// Maximum achievable acceleration/deceleration (m/s^2).
    pub max_acceleration: f64,
}
impl TrapezoidalConstraints {
    /// Generates a trapezoidal motion profile for a given distance.
    ///
    /// Returns a sequence of `MotionState` points, including acceleration/velocity
    /// at discrete time steps.
    ///
    /// The number of samples is fixed for simplicity in this initial implementation.
    /// In a real system, this would be determined by the control loop frequency.
    pub fn generate_profile(&self, total_distance: QLength) -> Vec<MotionState> {
        let distance = total_distance.as_meters();
        let max_v = self.max_velocity;
        let max_a = self.max_acceleration;
        let t_accel = max_v / max_a;
        let d_accel = 0.5 * max_a * t_accel.powi(2); // or max_v^2 / (2*max_a)
        let d_cruise = distance - 2.0 * d_accel;
        // no cruise
        if d_cruise < 0.0 {
            let v_peak_tri = sqrt(2.0 * max_a * distance);
            let t_total = 2.0 * (v_peak_tri / max_a);
            let samples = 100;
            let dt = t_total / (samples as f64 - 1.0);
            let mut states = Vec::with_capacity(samples);
            for i in 0..samples {
                let t = dt * i as f64;
                let pos_at_t = 0.5 * max_a * t.powi(2);
                let (velocity, acceleration, position) = if t <= t_total / 2.0 {
                    let v = max_a * t;
                    let a = max_a;
                    let p = 0.5 * max_a * t.powi(2);
                    (v, a, p)
                } else {
                    // Decelerating phase
                    let t_dec = t - t_total / 2.0;
                    let v = v_peak_tri - max_a * t_dec;
                    let a = -max_a;
                    let p_accel = d_accel;
                    let p_dec = v_peak_tri * t_dec - 0.5 * max_a * t_dec.powi(2);
                    (v, a, p_accel + p_dec)
                };
                states.push(MotionState {
                    time: QTime::from_sec(t),
                    position: QLength::from_meters(position),
                    velocity,
                    acceleration,
                });
            }
            return states;
        } else {
            // Case 2: Full Trapezoidal profile (with cruise phase)
            let t_cruise = d_cruise / max_v;
            let t_total = 2.0 * t_accel + t_cruise;
            let samples = 100; // Fixed sampling rate
            let dt = t_total / (samples as f64 - 1.0);
            let mut states = Vec::with_capacity(samples);
            for i in 0..samples {
                let t = dt * i as f64;
                let (velocity, acceleration, position) = if t < t_accel {
                    // Accelerating
                    let v = max_a * t;
                    let a = max_a;
                    let p = 0.5 * max_a * t.powi(2);
                    (v, a, p)
                } else if t < t_accel + t_cruise {
                    // Cruising
                    let t_cruise_elapsed = t - t_accel;
                    let v = max_v;
                    let a = 0.0;
                    let p = d_accel + max_v * t_cruise_elapsed;
                    (v, a, p)
                } else {
                    // Decelerating
                    let t_dec_elapsed = t - (t_accel + t_cruise);
                    let v = max_v - max_a * t_dec_elapsed;
                    let a = -max_a;
                    let p_accel_cruise = d_accel + max_v * t_cruise;
                    let p_dec = max_v * t_dec_elapsed - 0.5 * max_a * t_dec_elapsed.powi(2);
                    (v, a, p_accel_cruise + p_dec)
                };
                states.push(MotionState {
                    time: QTime::from_sec(t),
                    position: QLength::from_meters(position),
                    velocity,
                    acceleration,
                });
            }
            states
        }
    }
}