evian 0.2.0

Experimental command-based controls library for vexide.
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

use core::f64::consts::TAU;
use vexide::{core::float::Float, devices::smart::InertialSensor};

use crate::math::Vec2;

use super::{sensor::RotarySensor, wheel::TrackingWheel, Tracking, TrackingContext};

#[derive(Debug, PartialEq)]
pub struct ParallelWheelTracking<T: RotarySensor, U: RotarySensor> {
    position: Vec2,
    left_wheel: TrackingWheel<T>,
    right_wheel: TrackingWheel<U>,

    imu: Option<InertialSensor>,
    heading_offset: f64,
    prev_forward_travel: f64,
    prev_heading: f64,
}

impl<T: RotarySensor, U: RotarySensor> ParallelWheelTracking<T, U> {
    pub fn new(
        origin: Vec2,
        heading: f64,
        left_wheel: TrackingWheel<T>,
        right_wheel: TrackingWheel<U>,
        gyro: Option<InertialSensor>,
    ) -> Self {
        Self {
            position: origin,
            left_wheel,
            right_wheel,
            imu: gyro,
            heading_offset: heading,
            prev_forward_travel: 0.0,
            prev_heading: 0.0,
        }
    }

    pub fn set_heading(&mut self, heading: f64) {
        self.heading_offset = heading - self.heading();
    }

    pub fn set_position(&mut self, position: Vec2) {
        self.position = position;
    }

    pub fn track_width(&self) -> f64 {
        self.left_wheel.offset + self.right_wheel.offset
    }

    pub fn position(&self) -> Vec2 {
        self.position
    }

    pub fn heading(&self) -> f64 {
        (self.heading_offset
            + if let Some(imu) = &self.imu {
                if let Ok(heading) = imu.heading() {
                    TAU - heading.to_radians()
                } else {
                    (self.right_wheel.travel() - self.left_wheel.travel()) / self.track_width()
                }
            } else {
                (self.right_wheel.travel() - self.left_wheel.travel()) / self.track_width()
            })
            % TAU
    }

    pub fn forward_travel(&self) -> f64 {
        (self.left_wheel.travel() + self.right_wheel.travel()) / 2.0
    }
}

impl<T: RotarySensor, U: RotarySensor> Tracking for ParallelWheelTracking<T, U> {
    fn update(&mut self) -> TrackingContext {
        let forward_travel = self.forward_travel();
        let heading = self.heading();

        let delta_forward_travel = forward_travel - self.prev_forward_travel;
        let delta_heading = heading - self.prev_heading;
        let avg_heading = self.prev_heading + (delta_heading / 2.0);

        if delta_heading == 0.0 {
            self.position += Vec2::from_polar(delta_forward_travel, avg_heading);
        } else {
            self.position += Vec2::from_polar(
                2.0 * (delta_forward_travel / delta_heading) * (delta_heading / 2.0).sin(),
                avg_heading,
            );
        }

        self.prev_forward_travel = forward_travel;
        self.prev_heading = heading;

        TrackingContext {
            position: self.position,
            heading,
            forward_travel,
            linear_velocity: Default::default(),
            angular_velocity: Default::default(),
        }
    }
}