ozton-drivetrain 0.3.0

Drivetrain models for ozton.
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
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111

extern crate alloc;

use alloc::rc::Rc;
use core::cell::RefCell;

use glam::DVec2 as Vec2;
use ozton_math::desaturate;
use vexide::smart::{PortError, motor::Motor};

use super::{DrivetrainModel, Holonomic, Tank};

/// Mecanum drivetrain model.
pub struct Mecanum {
    /// Motors driving the front-left wheel(s).
    pub front_left_motors: Rc<RefCell<dyn AsMut<[Motor]>>>,

    /// Motors driving the front-right wheel(s).
    pub front_right_motors: Rc<RefCell<dyn AsMut<[Motor]>>>,

    /// Motors driving the back-left wheel(s).
    pub back_left_motors: Rc<RefCell<dyn AsMut<[Motor]>>>,

    /// Motors driving the back-right wheel(s).
    pub back_right_motors: Rc<RefCell<dyn AsMut<[Motor]>>>,
}

impl DrivetrainModel for Mecanum {
    type Error = PortError;
}

impl Holonomic for Mecanum {
    fn drive_vector(&mut self, vector: Vec2, turn: f64) -> Result<(), Self::Error> {
        let [fl, fr, bl, br] = desaturate(
            [
                vector.y + vector.x + turn,
                vector.y - vector.x - turn,
                vector.y - vector.x + turn,
                vector.y + vector.x - turn,
            ],
            1.0,
        );

        let mut rtn = Ok(());

        for motor in self.front_left_motors.borrow_mut().as_mut() {
            let result = motor.set_voltage(fl * motor.max_voltage());

            if result.is_err() {
                rtn = result;
            }
        }
        for motor in self.front_right_motors.borrow_mut().as_mut() {
            let result = motor.set_voltage(fr * motor.max_voltage());

            if result.is_err() {
                rtn = result;
            }
        }
        for motor in self.back_left_motors.borrow_mut().as_mut() {
            let result = motor.set_voltage(bl * motor.max_voltage());

            if result.is_err() {
                rtn = result;
            }
        }
        for motor in self.back_right_motors.borrow_mut().as_mut() {
            let result = motor.set_voltage(br * motor.max_voltage());

            if result.is_err() {
                rtn = result;
            }
        }

        rtn
    }
}

impl Tank for Mecanum {
    fn drive_tank(&mut self, left: f64, right: f64) -> Result<(), Self::Error> {
        let mut rtn = Ok(());

        for motor in self
            .front_left_motors
            .borrow_mut()
            .as_mut()
            .iter_mut()
            .chain(self.back_left_motors.borrow_mut().as_mut())
        {
            let result = motor.set_voltage(left * motor.max_voltage());

            if result.is_err() {
                rtn = result;
            }
        }
        for motor in self
            .front_right_motors
            .borrow_mut()
            .as_mut()
            .iter_mut()
            .chain(self.back_right_motors.borrow_mut().as_mut())
        {
            let result = motor.set_voltage(right * motor.max_voltage());

            if result.is_err() {
                rtn = result;
            }
        }

        rtn
    }
}