kernelvex 0.1.3

VEX Controls library
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65

use crate::dt::model::{Arcade, CurvatureDrive, Drivetrain, Tank};
use crate::util::utils::GroupErrors;
use crate::MotorGroup;

/// A differential (tank-style) drivetrain with left and right motor groups.
///
/// `DifferentialDrive` controls odom robot with independent left and right sides,
/// supporting tank, arcade, and curvature drive modes.
///
/// # Type Parameters
///
/// * `N` - The number of motors per side
pub struct DifferentialDrive<const N: usize> {
    left: MotorGroup<N>,
    right: MotorGroup<N>,
}

impl<const N: usize> DifferentialDrive<N> {
    #[inline]
    pub fn new(left: MotorGroup<N>, right: MotorGroup<N>) -> Self {
        Self { left, right }
    }
}

impl<const N: usize> Arcade for DifferentialDrive<N> {
    fn drive_arcade(&mut self, left: f64, right: f64) -> Result<(), GroupErrors> {
        let maximum = libm::fmax(libm::fabs(left), libm::fabs(right));
        let total = left + right;
        let difference = left - right;

        if left >= 0. {
            if right >= 0. {
                self.left.set_voltage(maximum)?;
                self.right.set_voltage(difference)?;
            } else {
                self.left.set_voltage(total)?;
                self.right.set_voltage(maximum)?;
            }
        } else if right >= 0. {
            self.left.set_voltage(total)?;
            self.right.set_voltage(-maximum)?;
        } else {
            self.left.set_voltage(-maximum)?;
            self.right.set_voltage(difference)?;
        }
        Ok(())
    }
}

impl<const N: usize> Tank for DifferentialDrive<N> {
    fn drive_tank(&mut self, left: f64, right: f64) -> Result<(), GroupErrors> {
        self.left.set_voltage(left * 12.0)?;
        self.right.set_voltage(right * 12.0)?;

        Ok(())
    }
}

impl<const N: usize> CurvatureDrive for DifferentialDrive<N> {
    fn drive_curvature(&mut self, left: f64, right: f64) -> Result<(), GroupErrors> {
        todo!()
    }
}

impl<const N: usize> Drivetrain for DifferentialDrive<N> {}