antaeus 0.3.7

A Versatile Framework 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
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
134
135
136
137
138
139
140
141

//! Point-and-shoot navigation using odometry.
//!
//! This module provides the [`PointShoot`] struct which combines odometry
//! tracking with PID control to enable simple point-to-point navigation.
//!
//! The "point-and-shoot" approach rotates to face the target, then drives
//! straight to it. This is simpler than arc-based pursuit but may be slower.
//!
//! # Example
//!
//! ```ignore
//! use antaeus::motion::odom::ptsht::PointShoot;
//! use antaeus::motion::odom::tracker::OdomTracker;
//! use antaeus::motion::pid::pid::PIDMovement;
//!
//! let point_shoot = PointShoot { odom, pid };
//!
//! // Navigate to a point
//! point_shoot.goto_point(24.0, 24.0).await;
//!
//! // Navigate to a pose (position + heading)
//! point_shoot.goto_pose(48.0, 0.0, 90.0).await;
//! ```

use crate::motion::{
    feedback_control::legacy_pid::linear_pid::PIDMovement,
    odom::tracker::OdomTracker,
};

/// Default timeout for movement operations in milliseconds.
const TIMEOUT: u64 = 10000;

/// Default delay after movement completion in milliseconds.
const AFTERDELAY: u64 = 10;

/// Point-and-shoot navigation controller.
///
/// Combines odometry tracking with PID control for simple point-to-point
/// navigation. The robot rotates to face the target, then drives straight
/// to it.
///
/// # Example
///
/// ```ignore
/// use antaeus::motion::odom::ptsht::PointShoot;
///
/// let point_shoot = PointShoot { odom, pid };
/// point_shoot.goto_point(24.0, 24.0).await;
/// ```
pub struct PointShoot {
    /// The odometry tracker for position feedback.
    pub odom: OdomTracker,
    /// The PID controller for movement execution.
    pub pid:  PIDMovement,
}

impl PointShoot {
    /// Creates a new PointShoot controller.
    ///
    /// # Arguments
    ///
    /// * `odom` - The odometry tracker for position feedback.
    /// * `pid` - The PID controller for movement execution.
    pub fn new(odom: OdomTracker, pid: PIDMovement) -> Self { Self { odom, pid } }

    /// Rotates the robot to face a specific point on the field.
    ///
    /// Calculates the angle to the target point and rotates the robot
    /// to face that direction using the IMU for feedback.
    ///
    /// # Arguments
    ///
    /// * `x` - Target X coordinate in inches.
    /// * `y` - Target Y coordinate in inches.
    pub async fn face_point(&self, x: f64, y: f64) {
        let delta_x = x - self.odom.global_pose.lock().await.x;
        let delta_y = y - self.odom.global_pose.lock().await.y;
        let angle = delta_y.atan2(delta_x).to_degrees();

        self.pid
            .rotate_imu(angle, &self.odom.trackermech.imu.clone(), TIMEOUT, AFTERDELAY)
            .await;
    }

    /// Moves the robot to a specific point on the field.
    ///
    /// First rotates to face the target, then drives straight to it.
    /// Uses the IMU for rotation feedback and motor encoders for distance.
    ///
    /// # Arguments
    ///
    /// * `x` - Target X coordinate in inches.
    /// * `y` - Target Y coordinate in inches.
    pub async fn goto_point(&self, x: f64, y: f64) {
        let delta_x = x - self.odom.global_pose.lock().await.x;
        let delta_y = y - self.odom.global_pose.lock().await.y;
        let angle = delta_y.atan2(delta_x).to_degrees();
        let hyp = (delta_x.powi(2) + delta_y.powi(2)).sqrt();

        self.pid
            .rotate_imu(angle, &self.odom.trackermech.imu.clone(), TIMEOUT, AFTERDELAY)
            .await;
        self.pid.travel(hyp, TIMEOUT, AFTERDELAY).await;
    }

    /// Moves the robot to a specific pose (position and heading).
    ///
    /// Executes three movements:
    /// 1. Rotate to face the target point.
    /// 2. Drive straight to the target point.
    /// 3. Rotate to the specified final heading.
    ///
    /// # Arguments
    ///
    /// * `x` - Target X coordinate in inches.
    /// * `y` - Target Y coordinate in inches.
    /// * `heading` - Final heading in degrees.
    pub async fn goto_pose(&self, x: f64, y: f64, heading: f64) {
        let delta_x = x - self.odom.global_pose.lock().await.x;
        let delta_y = y - self.odom.global_pose.lock().await.y;
        let angle = delta_y.atan2(delta_x).to_degrees();
        let hyp = (delta_x.powi(2) + delta_y.powi(2)).sqrt();
        self.pid
            .rotate_imu(angle, &self.odom.trackermech.imu.clone(), TIMEOUT, AFTERDELAY)
            .await;
        self.pid.travel(hyp, TIMEOUT, AFTERDELAY).await;
        self.pid.rotate(heading, TIMEOUT, AFTERDELAY).await;
    }

    /// Moves the robot forward in a straight line.
    ///
    /// Uses the PID controller to drive the specified distance.
    /// Positive values move forward, negative values move backward.
    ///
    /// # Arguments
    ///
    /// * `distance` - Distance to travel in inches.
    pub async fn travel(&self, distance: f64) {
        self.pid.travel(distance, TIMEOUT, AFTERDELAY).await;
    }
}