evian-motion 0.0.1-rc.1

Motion control algorithms in evian.
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

//! Feedback-driven driving and turning.

use core::time::Duration;

use evian_control::{Tolerances, loops::ControlLoop};
use evian_drivetrain::{Drivetrain, differential::Differential};
use evian_math::{Angle, Vec2};
use evian_tracking::{TracksForwardTravel, TracksHeading, TracksPosition, TracksVelocity};

mod distance_at_heading;
mod turn_to_point;

pub use distance_at_heading::DriveDistanceAtHeadingFuture;
pub use turn_to_point::TurnToPointFuture;

/// Feedback-driven driving and turning.
#[derive(PartialEq)]
pub struct Basic<
    L: ControlLoop<Input = f64, Output = f64> + Unpin + Clone,
    A: ControlLoop<Input = Angle, Output = f64> + Unpin + Clone,
> {
    /// Linear (forward driving) feedback controller.
    pub linear_controller: L,

    /// Angular (turning) feedback controller.
    pub angular_controller: A,

    /// Linear settling conditions.
    pub linear_tolerances: Tolerances,

    /// Angular settling conditions.
    pub angular_tolerances: Tolerances,

    /// Maximum duration the motion can take before being cancelled.
    pub timeout: Option<Duration>,
}

impl<
    L: ControlLoop<Input = f64, Output = f64> + Unpin + Clone,
    A: ControlLoop<Input = Angle, Output = f64> + Unpin + Clone,
> Basic<L, A>
{
    /// Moves the robot forwards by a given distance (measured in wheel units) while
    /// turning to face a heading.
    ///
    /// Negative `target_distance` values will move the robot backwards.
    pub fn drive_distance_at_heading<
        'a,
        T: TracksForwardTravel + TracksHeading + TracksVelocity,
    >(
        &mut self,
        drivetrain: &'a mut Drivetrain<Differential, T>,
        target_distance: f64,
        target_heading: Angle,
    ) -> DriveDistanceAtHeadingFuture<'a, L, A, T> {
        DriveDistanceAtHeadingFuture {
            target_distance,
            target_heading,
            timeout: self.timeout,
            linear_tolerances: self.linear_tolerances,
            angular_tolerances: self.angular_tolerances,
            linear_controller: self.linear_controller.clone(),
            angular_controller: self.angular_controller.clone(),
            drivetrain,
            state: None,
        }
    }

    /// Moves the robot forwards by a given distance (measured in wheel units).
    ///
    /// Negative `distance` values will move the robot backwards.
    pub fn drive_distance<'a, T: TracksForwardTravel + TracksHeading + TracksVelocity>(
        &mut self,
        drivetrain: &'a mut Drivetrain<Differential, T>,
        distance: f64,
    ) -> DriveDistanceAtHeadingFuture<'a, L, A, T> {
        self.drive_distance_at_heading(drivetrain, distance, drivetrain.tracking.heading())
    }

    /// Turns the robot in place to face a heading.
    pub fn turn_to_heading<'a, T: TracksForwardTravel + TracksHeading + TracksVelocity>(
        &mut self,
        drivetrain: &'a mut Drivetrain<Differential, T>,
        heading: Angle,
    ) -> DriveDistanceAtHeadingFuture<'a, L, A, T> {
        self.drive_distance_at_heading(drivetrain, 0.0, heading)
    }

    /// Turns the robot in place to face a 2D point.
    pub fn turn_to_point<
        'a,
        T: TracksForwardTravel + TracksPosition + TracksHeading + TracksVelocity,
    >(
        &mut self,
        drivetrain: &'a mut Drivetrain<Differential, T>,
        point: impl Into<Vec2<f64>>,
    ) -> TurnToPointFuture<'a, L, A, T> {
        TurnToPointFuture {
            point: point.into(),
            timeout: self.timeout,
            linear_tolerances: self.linear_tolerances,
            angular_tolerances: self.angular_tolerances,
            linear_controller: self.linear_controller.clone(),
            angular_controller: self.angular_controller.clone(),
            drivetrain,
            state: None,
        }
    }
}