kernelvex 0.1.2

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

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

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> {}