kernelvex/
differential.rs

1use crate::model::{Arcade, Tank, CurvatureDrive, Drivetrain};
2use crate::MotorGroup;
3use crate::utils::GroupErrors;
4
5pub struct DifferentialDrive<const N: usize> {
6    left: MotorGroup<N>,
7    right: MotorGroup<N>
8}
9
10
11impl<const N: usize> DifferentialDrive<N> {
12    #[inline]
13    fn new(left: MotorGroup<N>, right: MotorGroup<N>) -> Self {
14        Self { left, right }
15    }
16}
17
18impl<const N: usize> Arcade for DifferentialDrive<N> {
19    fn drive_arcade(&mut self, left: f64, right: f64) -> Result<(), GroupErrors> {
20        let maximum = libm::fmax(libm::fabs(left), libm::fabs(right));
21        let total = left + right;
22        let difference = left - right;
23
24        if left >= 0. {
25            if right >= 0. {
26                self.left.set_voltage(maximum)?;
27                self.right.set_voltage(difference)?;
28            } else {
29                self.left.set_voltage(total)?;
30                self.right.set_voltage(maximum)?;
31            }
32        }
33        else if right >= 0. {
34                  self.left.set_voltage(total)?;
35                  self.right.set_voltage(-maximum)?;
36              }
37              else {
38                  self.left.set_voltage(-maximum)?;
39                  self.right.set_voltage(difference)?;
40              }
41        Ok(())
42        }
43    }
44
45impl<const N: usize> Tank for DifferentialDrive<N> {
46    fn drive_tank(&mut self, left: f64, right: f64) -> Result<(), GroupErrors> {
47        self.left.set_voltage(left * 12.0)?;
48        self.right.set_voltage(right * 12.0)?;
49
50        Ok(())
51    }
52}
53
54impl<const N: usize> CurvatureDrive for DifferentialDrive<N> {
55    fn drive_curvature(&mut self, left: f64, right: f64) -> Result<(), GroupErrors> {
56        todo!()
57    }
58}
59
60impl<const N: usize> Drivetrain for DifferentialDrive<N> {}
kernelvex/differential.rs

kernelvex/
differential.rs
