ev3_drivebase/drivebase/
turn.rs

1use super::DriveBase;
2use crate::Direction;
3use ev3dev_lang_rust::Ev3Error;
4use std::f64::consts::PI;
5
6impl DriveBase {
7    #[expect(clippy::missing_errors_doc, missing_docs)]
8    pub fn turn(
9        &mut self,
10        speed: i32,
11        degree: i32,
12        radius: impl Into<Option<f64>>,
13    ) -> Result<&mut Self, Ev3Error> {
14        let mut radius: Option<f64> = radius.into();
15        radius = radius.map(f64::abs);
16
17        if degree == 0 {
18            return Ok(self);
19        }
20
21        match radius {
22            None => self.turn_in_place(speed, degree),
23            Some(radius) => self.turn_with_radius(speed, degree, radius),
24        }
25    }
26
27    #[expect(clippy::cast_possible_truncation)]
28    fn turn_in_place(&mut self, speed: i32, degree: i32) -> Result<&mut Self, Ev3Error> {
29        let speed = speed.abs();
30        let axle_radius = self.axle_track / 2.;
31        let arc_length = (axle_radius * f64::from(degree) * (PI / 180.))
32            .round()
33            .clamp(f64::from(i32::MIN), f64::from(i32::MAX)) as i32;
34
35        let direction = if degree >= 0 {
36            Direction::CounterClockwise
37        } else {
38            Direction::Clockwise
39        };
40
41        let counts = self.calculate_counts(arc_length, arc_length)?;
42        let (left_counts, right_counts) = match direction {
43            Direction::CounterClockwise => (-counts.0, counts.1),
44            Direction::Clockwise => (counts.0, -counts.1),
45        };
46
47        self.set_speed(speed, speed)?;
48
49        self.run_to_rel_pos(left_counts, right_counts)?;
50
51        self.wait_until_not_moving(None);
52
53        Ok(self)
54    }
55
56    #[expect(clippy::cast_possible_truncation)]
57    fn turn_with_radius(
58        &mut self,
59        mut speed: i32,
60        degree: i32,
61        radius: f64,
62    ) -> Result<&mut Self, Ev3Error> {
63        speed = speed.clamp(
64            -self.left.get_max_speed()? / 2,
65            self.left.get_max_speed()? / 2,
66        );
67
68        let theta = (f64::from(degree)).to_radians();
69        let theta_abs = theta.abs();
70        let half_track = self.axle_track / 2.0;
71        let eps = 1e-9;
72
73        let sign_f64 = |v: f64| if v >= 0.0 { 1 } else { -1 };
74        let deg_is_left = degree > 0;
75        let s = f64::from(speed);
76
77        let (left_speed_f, right_speed_f, left_arc_mm, right_arc_mm) = if radius.abs() < eps {
78            let arc = half_track * theta_abs;
79            let deg_sign = if deg_is_left { 1.0 } else { -1.0 };
80            let left_speed = -s * deg_sign;
81            let right_speed = s * deg_sign;
82            (left_speed, right_speed, arc, arc)
83        } else {
84            let r = radius.abs();
85            let (r_left, r_right) = if deg_is_left {
86                (r - half_track, r + half_track)
87            } else {
88                (r + half_track, r - half_track)
89            };
90
91            let left_speed = s * (r_left / r);
92            let right_speed = s * (r_right / r);
93
94            let left_arc = theta_abs * r_left.abs();
95            let right_arc = theta_abs * r_right.abs();
96            (left_speed, right_speed, left_arc, right_arc)
97        };
98
99        let left_rot = left_arc_mm / self.circumference;
100        let right_rot = right_arc_mm / self.circumference;
101
102        let left_tacho_deg = (left_rot * 360.0).round();
103        let right_tacho_deg = (right_rot * 360.0).round();
104
105        let left_speed =
106            (left_speed_f.round() as i32).saturating_mul(self.left_meta.direction.sign());
107        let right_speed =
108            (right_speed_f.round() as i32).saturating_mul(self.right_meta.direction.sign());
109
110        let left_counts = (left_tacho_deg as i32)
111            .saturating_mul(sign_f64(left_speed_f) * self.left_meta.direction.sign());
112        let right_counts = (right_tacho_deg as i32)
113            .saturating_mul(sign_f64(right_speed_f) * self.right_meta.direction.sign());
114
115        self.set_speed(left_speed, right_speed)?;
116        self.run_to_rel_pos(left_counts, right_counts)?;
117        self.wait_until_not_moving(None);
118
119        Ok(self)
120    }
121}
ev3_drivebase/drivebase/turn.rs

ev3_drivebase/drivebase/
turn.rs
