vex_v5_serial/
lib.rs

1//! Crate for interacting with the Vex V5 Robot brain. Not affiliated with Innovation First Inc.
2
3pub use vex_cdc as protocol;
4
5use std::{future::Future, time::Instant};
6
7use log::{error, trace, warn};
8use std::time::Duration;
9
10use vex_cdc::{
11    Decode, DecodeError, Encode, FixedStringSizeError, VarU16,
12    cdc::CdcReplyPacket,
13    cdc2::{Cdc2Ack, Cdc2ReplyPacket},
14};
15
16pub mod commands;
17
18use crate::commands::Command;
19
20#[cfg(feature = "bluetooth")]
21pub mod bluetooth;
22#[cfg(all(feature = "serial", feature = "bluetooth"))]
23pub mod generic;
24#[cfg(feature = "serial")]
25pub mod serial;
26
27pub trait CheckHeader {
28    fn has_valid_header(data: &[u8]) -> bool;
29}
30
31impl<const CMD: u8, const ECMD: u8, P: Decode> CheckHeader for Cdc2ReplyPacket<CMD, ECMD, P> {
32    fn has_valid_header(mut data: &[u8]) -> bool {
33        let data = &mut data;
34
35        if <[u8; 2] as Decode>::decode(data)
36            .map(|header| header != Self::HEADER)
37            .unwrap_or(true)
38        {
39            return false;
40        }
41
42        if u8::decode(data).map(|id| id != CMD).unwrap_or(true) {
43            return false;
44        }
45
46        let payload_size = VarU16::decode(data);
47        if payload_size.is_err() {
48            return false;
49        }
50
51        if u8::decode(data).map(|ecmd| ecmd != ECMD).unwrap_or(true) {
52            return false;
53        }
54
55        true
56    }
57}
58
59impl<const CMD: u8, P: Decode> CheckHeader for CdcReplyPacket<CMD, P> {
60    fn has_valid_header(data: &[u8]) -> bool {
61        let Some(data) = data.get(0..3) else {
62            return false;
63        };
64
65        data[0..2] == Self::HEADER && data[2] == CMD
66    }
67}
68
69#[derive(Debug, Clone, PartialEq, Eq)]
70pub(crate) struct RawPacket {
71    pub bytes: Vec<u8>,
72    pub used: bool,
73    pub timestamp: Instant,
74}
75impl RawPacket {
76    pub fn new(bytes: Vec<u8>) -> Self {
77        Self {
78            bytes,
79            used: false,
80            timestamp: Instant::now(),
81        }
82    }
83
84    pub fn is_obsolete(&self, timeout: Duration) -> bool {
85        self.timestamp.elapsed() > timeout || self.used
86    }
87
88    pub fn check_header<H: CheckHeader>(&self) -> bool {
89        H::has_valid_header(&self.bytes)
90    }
91
92    /// Decodes the packet into the given type.
93    /// If successful, marks the packet as used.
94    /// # Note
95    /// This function will **NOT** fail if the packet has already been used.
96    pub fn decode_and_use<D: Decode>(&mut self) -> Result<D, DecodeError> {
97        let decoded = D::decode(&mut self.bytes.as_slice())?;
98        self.used = true;
99        Ok(decoded)
100    }
101}
102/// Removes old and used packets from the incoming packets buffer.
103pub(crate) fn trim_packets(packets: &mut Vec<RawPacket>) {
104    trace!("Trimming packets. Length before: {}", packets.len());
105
106    // Remove packets that are obsolete
107    packets.retain(|packet| !packet.is_obsolete(Duration::from_secs(2)));
108
109    trace!("Trimmed packets. Length after: {}", packets.len());
110}
111
112/// Represents an open connection to a V5 peripheral.
113#[allow(async_fn_in_trait)]
114pub trait Connection {
115    type Error: std::error::Error + From<DecodeError> + From<Cdc2Ack> + From<FixedStringSizeError>;
116
117    fn connection_type(&self) -> ConnectionType;
118
119    /// Sends a packet.
120    fn send(&mut self, packet: impl Encode) -> impl Future<Output = Result<(), Self::Error>>;
121
122    /// Receives a packet.
123    fn recv<P: Decode + CheckHeader>(
124        &mut self,
125        timeout: Duration,
126    ) -> impl Future<Output = Result<P, Self::Error>>;
127
128    /// Read user program output.
129    fn read_user(&mut self, buf: &mut [u8]) -> impl Future<Output = Result<usize, Self::Error>>;
130
131    /// Write to user program stdio.
132    fn write_user(&mut self, buf: &[u8]) -> impl Future<Output = Result<usize, Self::Error>>;
133
134    /// Executes a [`Command`].
135    fn execute_command<C: Command>(
136        &mut self,
137        command: C,
138    ) -> impl Future<Output = Result<C::Output, Self::Error>> {
139        command.execute(self)
140    }
141
142    /// Sends a packet and waits for a response.
143    ///
144    /// This function will retry the handshake `retries` times
145    /// before giving up and erroring with the error thrown on the last retry.
146    ///
147    /// # Note
148    ///
149    /// This function will fail immediately if the given packet fails to encode.
150    async fn handshake<D: Decode + CheckHeader>(
151        &mut self,
152        timeout: Duration,
153        retries: usize,
154        packet: impl Encode + Clone,
155    ) -> Result<D, Self::Error> {
156        let mut last_error = None;
157
158        for _ in 0..=retries {
159            self.send(packet.clone()).await?;
160            match self.recv::<D>(timeout).await {
161                Ok(decoded) => return Ok(decoded),
162                Err(e) => {
163                    warn!(
164                        "Handshake failed while waiting for {}: {:?}. Retrying...",
165                        std::any::type_name::<D>(),
166                        e
167                    );
168                    last_error = Some(e);
169                }
170            }
171        }
172        error!(
173            "Handshake failed after {} retries with error: {:?}",
174            retries, last_error
175        );
176        Err(last_error.unwrap())
177    }
178}
179
180#[derive(Debug, Clone, Copy, Eq, PartialEq)]
181pub enum ConnectionType {
182    Wired,
183    Controller,
184    Bluetooth,
185}
186impl ConnectionType {
187    pub fn is_wired(&self) -> bool {
188        matches!(self, ConnectionType::Wired)
189    }
190    pub fn is_controller(&self) -> bool {
191        matches!(self, ConnectionType::Controller)
192    }
193    pub fn is_bluetooth(&self) -> bool {
194        matches!(self, ConnectionType::Bluetooth)
195    }
196
197    pub(crate) fn max_chunk_size(&self, window_size: u16) -> u16 {
198        const USER_PROGRAM_CHUNK_SIZE: u16 = 4096;
199
200        #[cfg(feature = "bluetooth")]
201        {
202            use crate::bluetooth::BluetoothConnection;
203
204            if self.is_bluetooth() {
205                let max_chunk_size =
206                    (BluetoothConnection::MAX_PACKET_SIZE as u16).min(window_size / 2) - 14;
207                max_chunk_size - (max_chunk_size % 4)
208            } else if window_size > 0 && window_size <= USER_PROGRAM_CHUNK_SIZE {
209                window_size
210            } else {
211                USER_PROGRAM_CHUNK_SIZE
212            }
213        }
214
215        #[cfg(not(feature = "bluetooth"))]
216        if window_size > 0 && window_size <= USER_PROGRAM_CHUNK_SIZE {
217            window_size
218        } else {
219            USER_PROGRAM_CHUNK_SIZE
220        }
221    }
222}
vex_v5_serial/lib.rs

vex_v5_serial/
lib.rs
