kernelvex 0.1.3

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
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133

use kernelvex::motion::trajectory::{Trajectory, TrajectoryPoint};
use kernelvex::odom::pose::Pose;
use kernelvex::util::si::{QAngle, QTime};

#[test]
fn test_trajectory_sample_endpoints() {
    let mut traj = Trajectory::new();
    traj.push(TrajectoryPoint::new(
        Pose::new(0.0, 0.0, QAngle::from_degrees(0.0)),
        0.0,
        0.0,
        QTime::from_sec(0.0),
    ));
    traj.push(TrajectoryPoint::new(
        Pose::new(1.0, 0.0, QAngle::from_degrees(0.0)),
        1.0,
        0.0,
        QTime::from_sec(1.0),
    ));

    let start = traj.sample(QTime::from_sec(0.0)).unwrap();
    let end = traj.sample(QTime::from_sec(1.0)).unwrap();

    let (sx, sy) = start.pose.position();
    let (ex, ey) = end.pose.position();

    assert!((sx - 0.0).abs() < 1e-6);
    assert!((sy - 0.0).abs() < 1e-6);
    assert!((ex - 1.0).abs() < 1e-6);
    assert!((ey - 0.0).abs() < 1e-6);
}

#[test]
fn test_trajectory_sample_intermediate() {
    let mut traj = Trajectory::new();
    traj.push(TrajectoryPoint::new(
        Pose::new(0.0, 0.0, QAngle::from_degrees(0.0)),
        0.0,
        0.0,
        QTime::from_sec(0.0),
    ));
    traj.push(TrajectoryPoint::new(
        Pose::new(2.0, 0.0, QAngle::from_degrees(0.0)),
        2.0,
        0.0,
        QTime::from_sec(2.0),
    ));

    let mid = traj.sample(QTime::from_sec(1.0)).unwrap();
    let (mx, my) = mid.pose.position();

    assert!((mx - 1.0).abs() < 1e-6);
    assert!((my - 0.0).abs() < 1e-6);
    assert!((mid.linear_velocity - 1.0).abs() < 1e-6);
}

#[test]
fn test_trajectory_sample_out_of_bounds() {
    let mut traj = Trajectory::new();
    traj.push(TrajectoryPoint::new(
        Pose::new(0.0, 0.0, QAngle::from_degrees(0.0)),
        0.0,
        0.0,
        QTime::from_sec(0.0),
    ));
    traj.push(TrajectoryPoint::new(
        Pose::new(1.0, 0.0, QAngle::from_degrees(0.0)),
        1.0,
        0.0,
        QTime::from_sec(1.0),
    ));

    let before = traj.sample(QTime::from_sec(-1.0)).unwrap();
    let after = traj.sample(QTime::from_sec(2.0)).unwrap();

    let (bx, by) = before.pose.position();
    let (ax, ay) = after.pose.position();

    assert!((bx - 0.0).abs() < 1e-6);
    assert!((by - 0.0).abs() < 1e-6);
    assert!((ax - 1.0).abs() < 1e-6);
    assert!((ay - 0.0).abs() < 1e-6);
}

#[test]
fn test_trajectory_heading_interpolation_shortest_arc() {
    let mut traj = Trajectory::new();
    traj.push(TrajectoryPoint::new(
        Pose::new(0.0, 0.0, QAngle::from_degrees(350.0)),
        0.0,
        0.0,
        QTime::from_sec(0.0),
    ));
    traj.push(TrajectoryPoint::new(
        Pose::new(0.0, 0.0, QAngle::from_degrees(10.0)),
        0.0,
        0.0,
        QTime::from_sec(1.0),
    ));

    let mid = traj.sample(QTime::from_sec(0.5)).unwrap();
    let heading = mid.pose.heading().remainder(QAngle::TAU).as_degrees();
    let heading_abs = heading.abs();

    assert!(heading_abs < 1e-6 || (360.0 - heading_abs).abs() < 1e-6);
}

#[test]
fn test_trajectory_total_time() {
    let traj = Trajectory::from_points(vec![
        TrajectoryPoint::new(
            Pose::new(0.0, 0.0, QAngle::from_degrees(0.0)),
            0.0,
            0.0,
            QTime::from_sec(0.0),
        ),
        TrajectoryPoint::new(
            Pose::new(1.0, 0.0, QAngle::from_degrees(0.0)),
            1.0,
            0.0,
            QTime::from_sec(3.0),
        ),
    ]);

    let total = traj.total_time().unwrap();
    assert!((total.as_sec() - 3.0).abs() < 1e-6);
}

#[test]
fn test_trajectory_sample_empty() {
    let traj = Trajectory::new();
    assert!(traj.sample(QTime::from_sec(0.0)).is_none());
}