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

use kernelvex::PurePursuit;
use kernelvex::{Trajectory, TrajectoryPoint};
use kernelvex::Pose;
use kernelvex::{QAngle, QTime, Vector2};

#[test]
fn test_pure_pursuit_lookahead_on_line() {
    let mut traj = Trajectory::new();
    traj.push(TrajectoryPoint::new(
        Pose::new(Default::default(), QAngle::from_degrees(0.0)),
        1.0,
        0.0,
        QTime::from_sec(0.0),
    ));
    traj.push(TrajectoryPoint::new(
        Pose::new(Vector2::<f64>::new(2., 0.), QAngle::from_degrees(0.0)),
        1.0,
        0.0,
        QTime::from_sec(2.0),
    ));

    let pp = PurePursuit::new(traj, 1.0);
    let pose = Pose::new(Default::default(), QAngle::from_degrees(0.0));
    let target = pp.intersect(pose).expect("lookahead point");

    let pos = target.pose.position();
    assert!((pos.x - 1.0).abs() < 1e-9);
    assert!(pos.y.abs() < 1e-9);
}

#[test]
fn test_pure_pursuit_tangent() {
    let mut traj = Trajectory::new();
    traj.push(TrajectoryPoint::new(
        Pose::new(Vector2::<f64>::new(-18., 0.), QAngle::from_degrees(0.0)),
        1.0,
        0.0,
        QTime::from_sec(0.0),
    ));
    traj.push(TrajectoryPoint::new(
        Pose::new(Vector2::<f64>::new(22., 30.), QAngle::from_degrees(0.0)),
        1.0,
        0.0,
        QTime::from_sec(2.0),
    ));
    let pp = PurePursuit::new(traj, 5.0);
    let pose = Pose::new(Vector2::<f64>::new(5., 11.), QAngle::from_degrees(0.0));
    let target = pp.intersect(pose).expect("lookahead point");
    let pos = target.pose.position();
    assert_eq!(pos.x, 2.);
    assert_eq!(pos.y, 15.);
}

#[test]
fn test_pure_pursuit_curvature_sign() {
    let traj = Trajectory::from_points(vec![TrajectoryPoint::new(
        Pose::new(Default::default(), QAngle::from_degrees(0.0)),
        1.0,
        0.0,
        QTime::from_sec(0.0),
    )]);
    let pp = PurePursuit::new(traj, 1.0);

    let pose = Pose::new(Default::default(), QAngle::from_degrees(0.0));
    let target = Vector2::<f64>::new(1.0, 1.0);
    let curvature = pp.curvature(pose, target);

    assert!(curvature > 0.0);
}