omnitiles/control/mecanum.rs
1// SPDX-License-Identifier: MIT
2// © 2025–2026 Christopher Liu
3
4//! Mecanum wheel inverse kinematics.
5//!
6//! Wheel layout (top-down view, rollers shown as `/` and `\`):
7//!
8//! ```text
9//! FL \/ /\ FR +X (forward)
10//! ^
11//! RL /\ \/ RR | +omega (CCW)
12//! +---> +Y (left)
13//! ```
14
15/// Compute per-wheel speeds from body-frame velocity commands.
16///
17/// Returns `[front_left, front_right, rear_left, rear_right]`, each in -1.0..1.0.
18/// If any raw speed exceeds 1.0, all four are scaled down proportionally to
19/// preserve the commanded direction.
20pub fn mecanum_ik(vx: f32, vy: f32, omega: f32) -> [f32; 4] {
21 let fl = vx - vy - omega;
22 let fr = vx + vy + omega;
23 let rl = vx + vy - omega;
24 let rr = vx - vy + omega;
25
26 let mut speeds = [fl, fr, rl, rr];
27
28 // Proportional desaturation
29 let mut max_abs: f32 = 1.0;
30 for &s in &speeds {
31 let a = if s < 0.0 { -s } else { s };
32 if a > max_abs {
33 max_abs = a;
34 }
35 }
36 if max_abs > 1.0 {
37 for s in &mut speeds {
38 *s /= max_abs;
39 }
40 }
41
42 speeds
43}