pamoja_kit/
drive.rs

1//! Differential-drive wheel kinematics.
2
3/// Converts between a robot's motion and its two wheel speeds (differential drive).
4///
5/// A differential-drive robot steers by spinning its left and right wheels at different
6/// speeds. This converts both ways: [`wheel_speeds`](DiffDrive::wheel_speeds) turns a desired
7/// forward speed and turn rate into the wheel speeds to command (inverse kinematics), and
8/// [`body_motion`](DiffDrive::body_motion) turns measured wheel speeds back into the robot's
9/// forward speed and turn rate (forward kinematics). The one parameter is the track: the
10/// distance between the wheels.
11///
12/// # Examples
13///
14/// ```
15/// use pamoja_kit::DiffDrive;
16///
17/// let drive = DiffDrive::new(0.5); // wheels 0.5 apart
18/// // Drive straight: both wheels turn at the forward speed.
19/// assert_eq!(drive.wheel_speeds(1.0, 0.0), (1.0, 1.0));
20/// // Spin in place: the wheels turn opposite, each at turn rate times half the track.
21/// assert_eq!(drive.wheel_speeds(0.0, 2.0), (-0.5, 0.5));
22/// ```
23#[derive(Clone, Copy, Debug)]
24pub struct DiffDrive {
25    track: f32,
26}
27
28impl DiffDrive {
29    /// Creates a model for wheels `track` apart.
30    ///
31    /// # Arguments
32    ///
33    /// * `track` - the distance between the left and right wheels; its magnitude is used.
34    ///
35    /// # Returns
36    ///
37    /// The kinematics model.
38    pub fn new(track: f32) -> Self {
39        Self {
40            track: magnitude(track),
41        }
42    }
43
44    /// Returns the `(left, right)` wheel speeds for a desired body motion.
45    ///
46    /// # Arguments
47    ///
48    /// * `linear` - the forward speed.
49    /// * `angular` - the turn rate, positive turning toward the left (counter-clockwise).
50    ///
51    /// # Returns
52    ///
53    /// `(left, right)`, where `left = linear - angular * track / 2` and
54    /// `right = linear + angular * track / 2`.
55    pub fn wheel_speeds(&self, linear: f32, angular: f32) -> (f32, f32) {
56        let half = angular * self.track / 2.0;
57        (linear - half, linear + half)
58    }
59
60    /// Returns the body `(linear, angular)` motion for measured wheel speeds.
61    ///
62    /// # Arguments
63    ///
64    /// * `left` - the left wheel speed.
65    /// * `right` - the right wheel speed.
66    ///
67    /// # Returns
68    ///
69    /// `(linear, angular)`, where `linear = (right + left) / 2` and
70    /// `angular = (right - left) / track`. Angular is zero when the track is zero.
71    pub fn body_motion(&self, left: f32, right: f32) -> (f32, f32) {
72        let linear = (right + left) / 2.0;
73        let angular = if self.track == 0.0 {
74            0.0
75        } else {
76            (right - left) / self.track
77        };
78        (linear, angular)
79    }
80}
81
82// `f32::abs` lives in `std`, so this `no_std` crate takes the magnitude by hand.
83fn magnitude(value: f32) -> f32 {
84    if value < 0.0 {
85        -value
86    } else {
87        value
88    }
89}
90
91#[cfg(test)]
92mod tests {
93    use super::*;
94
95    #[test]
96    fn driving_straight_turns_both_wheels_equally() {
97        let drive = DiffDrive::new(0.5);
98        assert_eq!(drive.wheel_speeds(1.0, 0.0), (1.0, 1.0));
99    }
100
101    #[test]
102    fn spinning_in_place_turns_the_wheels_opposite() {
103        let drive = DiffDrive::new(0.5);
104        assert_eq!(drive.wheel_speeds(0.0, 2.0), (-0.5, 0.5));
105    }
106
107    #[test]
108    fn body_motion_inverts_wheel_speeds() {
109        let drive = DiffDrive::new(0.5);
110        assert_eq!(drive.body_motion(1.0, 1.0), (1.0, 0.0)); // straight
111        assert_eq!(drive.body_motion(-0.5, 0.5), (0.0, 2.0)); // spinning
112    }
113
114    #[test]
115    fn the_two_directions_round_trip() {
116        let drive = DiffDrive::new(0.42);
117        let (left, right) = drive.wheel_speeds(1.3, -0.7);
118        let (linear, angular) = drive.body_motion(left, right);
119        assert!((linear - 1.3).abs() < 1e-6);
120        assert!((angular + 0.7).abs() < 1e-6);
121    }
122
123    #[test]
124    fn a_zero_track_reports_no_rotation() {
125        let drive = DiffDrive::new(0.0);
126        assert_eq!(drive.body_motion(1.0, 2.0), (1.5, 0.0));
127    }
128}
pamoja_kit/drive.rs

pamoja_kit/
drive.rs
