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

use core::f64::consts::{FRAC_PI_2, FRAC_PI_4};

use vexide::{core::time::Instant, devices::smart::Motor, prelude::Float};

use crate::{
    command::{settler::Settler, Command, CommandUpdate},
    control::Feedback,
    differential::Voltages,
    math::Vec2,
    tracking::TrackingContext,
};

#[derive(Clone, Copy, PartialEq)]
struct MoveToPoint<F: Feedback<Error = f64, Output = f64>> {
    target: Vec2,

    distance_controller: F,
    angle_controller: F,

    settler: Settler,

    prev_timestamp: Instant,
}

impl<F: Feedback<Error = f64, Output = f64>> Command for MoveToPoint<F> {
    type Output = Voltages;

    fn update(&mut self, cx: TrackingContext) -> CommandUpdate<Self::Output> {
        let dt = self.prev_timestamp.elapsed();

        let local_target = self.target - cx.position;

        let mut distance_error = local_target.length();
        let mut angle_error = (cx.heading - local_target.angle()) % FRAC_PI_2;

        if self.settler.is_settled(distance_error, cx.linear_velocity) {
            return CommandUpdate::Settled;
        }

        if angle_error.abs() > FRAC_PI_4 {
            distance_error = -distance_error;
            angle_error -= FRAC_PI_2;
        }

        let angular_output = self.angle_controller.update(angle_error, dt);
        let linear_output = self.distance_controller.update(distance_error, dt) * angle_error.cos();

        self.prev_timestamp = Instant::now();

        CommandUpdate::Update(
            Voltages(
                linear_output + angular_output,
                linear_output - angular_output,
            )
            .normalized(Motor::MAX_VOLTAGE),
        )
    }
}