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

//! ADI ultrasonic sensor.

use pros_core::bail_on;
use pros_sys::{ext_adi_ultrasonic_t, PROS_ERR};

use super::{AdiDevice, AdiDeviceType, AdiError, AdiPort};

#[derive(Debug, Eq, PartialEq)]
/// Adi ultrasonic sensor.
/// Requires two ports one for pinging, and one for listening for the response.
pub struct AdiUltrasonic {
    raw: ext_adi_ultrasonic_t,
    port_ping: AdiPort,
    port_echo: AdiPort,
}

impl AdiUltrasonic {
    /// Create a new ultrasonic sensor from a ping and echo [`AdiPort`].
    pub fn new(ports: (AdiPort, AdiPort)) -> Result<Self, AdiError> {
        let port_ping = ports.0;
        let port_echo = ports.1;

        if port_ping.internal_expander_index() != port_echo.internal_expander_index() {
            return Err(AdiError::ExpanderPortMismatch);
        }

        let raw = bail_on!(PROS_ERR, unsafe {
            pros_sys::ext_adi_ultrasonic_init(
                port_ping.internal_expander_index(),
                port_ping.index(),
                port_echo.index(),
            )
        });

        Ok(Self {
            raw,
            port_ping,
            port_echo,
        })
    }

    /// Get the distance reading of the ultrasonic sensor in centimeters.
    ///
    /// Round and/or fluffy objects can cause inaccurate values to be returned.
    pub fn distance(&self) -> Result<u16, AdiError> {
        Ok(bail_on!(PROS_ERR, unsafe {
            pros_sys::ext_adi_ultrasonic_get(self.raw)
        }) as u16)
    }
}

impl AdiDevice for AdiUltrasonic {
    type PortIndexOutput = (u8, u8);

    fn port_index(&self) -> Self::PortIndexOutput {
        (self.port_ping.index(), self.port_echo.index())
    }

    fn expander_port_index(&self) -> Option<u8> {
        self.port_ping.expander_index()
    }

    fn device_type(&self) -> AdiDeviceType {
        AdiDeviceType::LegacyUltrasonic
    }
}