omnitiles/control/
base_controller.rs

1// SPDX-License-Identifier: MIT
2// © 2025–2026 Christopher Liu
3
4//! Four-wheel mecanum base controller.
5//!
6//! Wraps four `Tb6612` drivers and applies mecanum inverse kinematics to
7//! convert body-frame velocity commands into per-wheel speeds.
8
9use crate::control::mecanum;
10use crate::drivers::tb6612::Tb6612;
11
12use stm32f7xx_hal::prelude::*;
13
14pub struct BaseController<
15    const W1_IN1_P: char,
16    const W1_IN1_N: u8,
17    const W1_IN2_P: char,
18    const W1_IN2_N: u8,
19    W1Pwm,
20    const W2_IN1_P: char,
21    const W2_IN1_N: u8,
22    const W2_IN2_P: char,
23    const W2_IN2_N: u8,
24    W2Pwm,
25    const W3_IN1_P: char,
26    const W3_IN1_N: u8,
27    const W3_IN2_P: char,
28    const W3_IN2_N: u8,
29    W3Pwm,
30    const W4_IN1_P: char,
31    const W4_IN1_N: u8,
32    const W4_IN2_P: char,
33    const W4_IN2_N: u8,
34    W4Pwm,
35> {
36    pub fl: Tb6612<W1_IN1_P, W1_IN1_N, W1_IN2_P, W1_IN2_N, W1Pwm>,
37    pub fr: Tb6612<W2_IN1_P, W2_IN1_N, W2_IN2_P, W2_IN2_N, W2Pwm>,
38    pub rl: Tb6612<W3_IN1_P, W3_IN1_N, W3_IN2_P, W3_IN2_N, W3Pwm>,
39    pub rr: Tb6612<W4_IN1_P, W4_IN1_N, W4_IN2_P, W4_IN2_N, W4Pwm>,
40}
41
42impl<
43        const W1_IN1_P: char,
44        const W1_IN1_N: u8,
45        const W1_IN2_P: char,
46        const W1_IN2_N: u8,
47        W1Pwm: _embedded_hal_PwmPin<Duty = u16>,
48        const W2_IN1_P: char,
49        const W2_IN1_N: u8,
50        const W2_IN2_P: char,
51        const W2_IN2_N: u8,
52        W2Pwm: _embedded_hal_PwmPin<Duty = u16>,
53        const W3_IN1_P: char,
54        const W3_IN1_N: u8,
55        const W3_IN2_P: char,
56        const W3_IN2_N: u8,
57        W3Pwm: _embedded_hal_PwmPin<Duty = u16>,
58        const W4_IN1_P: char,
59        const W4_IN1_N: u8,
60        const W4_IN2_P: char,
61        const W4_IN2_N: u8,
62        W4Pwm: _embedded_hal_PwmPin<Duty = u16>,
63    >
64    BaseController<
65        W1_IN1_P,
66        W1_IN1_N,
67        W1_IN2_P,
68        W1_IN2_N,
69        W1Pwm,
70        W2_IN1_P,
71        W2_IN1_N,
72        W2_IN2_P,
73        W2_IN2_N,
74        W2Pwm,
75        W3_IN1_P,
76        W3_IN1_N,
77        W3_IN2_P,
78        W3_IN2_N,
79        W3Pwm,
80        W4_IN1_P,
81        W4_IN1_N,
82        W4_IN2_P,
83        W4_IN2_N,
84        W4Pwm,
85    >
86{
87    pub fn new(
88        fl: Tb6612<W1_IN1_P, W1_IN1_N, W1_IN2_P, W1_IN2_N, W1Pwm>,
89        fr: Tb6612<W2_IN1_P, W2_IN1_N, W2_IN2_P, W2_IN2_N, W2Pwm>,
90        rl: Tb6612<W3_IN1_P, W3_IN1_N, W3_IN2_P, W3_IN2_N, W3Pwm>,
91        rr: Tb6612<W4_IN1_P, W4_IN1_N, W4_IN2_P, W4_IN2_N, W4Pwm>,
92    ) -> Self {
93        Self { fl, fr, rl, rr }
94    }
95
96    /// Set body-frame velocity. Inputs are normalized: -1.0..1.0 for each axis.
97    pub fn set_velocity(&mut self, vx: f32, vy: f32, omega: f32) {
98        let [fl, fr, rl, rr] = mecanum::mecanum_ik(vx, vy, omega);
99        self.fl.set_speed(-fl);
100        self.fr.set_speed(fr);
101        self.rl.set_speed(-rl);
102        self.rr.set_speed(rr);
103    }
104
105    /// Brake all four wheels.
106    pub fn brake(&mut self) {
107        self.fl.brake();
108        self.fr.brake();
109        self.rl.brake();
110        self.rr.brake();
111    }
112}
omnitiles/control/base_controller.rs

omnitiles/control/
base_controller.rs
