fll_rs/movement/
spec.rs

1use std::f32::consts::PI;
2
3use crate::types::{
4    Acceleration, Degrees, DegreesPerSecond, DegreesPerSecondPerSecond, Distance, Heading,
5    Milimeters, Speed, UnitsExt,
6};
7
8#[derive(Clone)]
9pub struct RobotSpec {
10    acceleration: DegreesPerSecondPerSecond,
11    deceleration: DegreesPerSecondPerSecond,
12
13    wheel_circumference: Milimeters,
14    wheel_diameter: Milimeters,
15    wheelbase_circumference: Milimeters,
16    wheelbase_diameter: Milimeters,
17
18    max_speed: DegreesPerSecond,
19}
20
21impl RobotSpec {
22    pub fn new(
23        acceleration: impl Into<Acceleration>,
24        deceleration: impl Into<Acceleration>,
25        wheel_diameter: Milimeters,
26        wheelbase_diameter: Milimeters,
27        max_speed: impl Into<Speed>,
28    ) -> Self {
29        let wheel_circumference = PI * wheel_diameter.0;
30        let wheelbase_circumference = PI * wheelbase_diameter.0 * 2.0;
31
32        let preliminary_spec = RobotSpec {
33            acceleration: DegreesPerSecondPerSecond(0.0),
34            deceleration: DegreesPerSecondPerSecond(0.0),
35            wheel_circumference: wheel_circumference.mm(),
36            wheel_diameter,
37            wheelbase_circumference: wheelbase_circumference.mm(),
38            wheelbase_diameter,
39            max_speed: DegreesPerSecond(0.0),
40        };
41
42        RobotSpec {
43            acceleration: acceleration.into().to_dps2(&preliminary_spec),
44            deceleration: deceleration.into().to_dps2(&preliminary_spec),
45            max_speed: max_speed.into().to_dps(&preliminary_spec),
46            ..preliminary_spec
47        }
48    }
49
50    /// The acceleration rate used for movement
51    /// In degrees per second per second
52    pub fn acceleration(&self) -> DegreesPerSecondPerSecond {
53        self.acceleration
54    }
55
56    /// The deceleration rate used for movement
57    /// In degrees per second per second
58    pub fn deceleration(&self) -> DegreesPerSecondPerSecond {
59        self.deceleration
60    }
61
62    /// The circumference of the main wheels on the robot
63    /// In millimeters
64    pub fn wheel_circumference(&self) -> Milimeters {
65        self.wheel_circumference
66    }
67
68    /// The diameter of the main wheels on the robot
69    /// In millimeters
70    pub fn wheel_diameter(&self) -> Milimeters {
71        self.wheel_diameter
72    }
73
74    /// The turning circumference of the robot's wheelbase
75    /// In millimeters
76    pub fn wheelbase_circumference(&self) -> Milimeters {
77        self.wheelbase_circumference
78    }
79
80    /// The distance between the robot's 2 wheels
81    /// In millimeters
82    pub fn wheelbase_diameter(&self) -> Milimeters {
83        self.wheelbase_diameter
84    }
85
86    /// The max supported driving speed
87    /// In degrees per second
88    pub fn max_speed(&self) -> DegreesPerSecond {
89        self.max_speed
90    }
91
92    /// Calculated the amount of degrees a wheel would need to turn to cause the robot to turn
93    /// `angle` degrees
94    ///
95    /// Assumes the robot has a heading of 0
96    /// The actual heading should be subtracted from `angle`
97    pub fn get_distance_for_turn(&self, Heading(angle): Heading) -> Degrees {
98        (angle * self.wheelbase_circumference.0 / self.wheel_circumference.0).deg()
99    }
100
101    /// Calculated the direction the robot should be facing based on the wheel angles
102    pub fn get_approx_angle(
103        &self,
104        left: impl Into<Distance>,
105        right: impl Into<Distance>,
106    ) -> Heading {
107        ((left.into().to_deg(&self).0 - right.into().to_deg(&self).0) * self.wheel_circumference.0
108            / self.wheelbase_circumference.0)
109            .ang()
110    }
111
112    /// Converts wheel degrees to millimeters
113    pub fn deg_to_mm(&self, Degrees(deg): Degrees) -> Milimeters {
114        (deg / 360.0 * self.wheel_circumference.0).mm()
115    }
116
117    /// Converts millimeters to wheel degrees
118    pub fn mm_to_deg(&self, Milimeters(mm): Milimeters) -> Degrees {
119        (mm / self.wheel_circumference.0 * 360.0).deg()
120    }
121}
fll_rs/movement/spec.rs

fll_rs/movement/
spec.rs
