vex_rt/
controller.rs

1//! Controller API.
2
3use alloc::collections::VecDeque;
4use core::{convert::TryInto, fmt, time::Duration};
5use slice_copy::copy;
6
7use crate::{
8    bindings,
9    error::{get_errno, Error},
10    io::eprintln,
11    rtos::{delay_until, queue, time_since_start, DataSource, SendQueue, Task},
12    select,
13};
14
15const SCREEN_SUCCESS_DELAY: Duration = Duration::from_millis(50);
16const SCREEN_FAILURE_DELAY: Duration = Duration::from_millis(5);
17
18/// Represents a Vex controller.
19pub struct Controller {
20    id: bindings::controller_id_e_t,
21    /// The left analog stick.
22    pub left_stick: AnalogStick,
23    /// The right analog stick.
24    pub right_stick: AnalogStick,
25    /// The top-left shoulder button.
26    pub l1: Button,
27    /// The bottom-left shoulder button.
28    pub l2: Button,
29    /// The top-right shoulder button.
30    pub r1: Button,
31    /// The bottom-right shoulder button.
32    pub r2: Button,
33    /// The up directional button.
34    pub up: Button,
35    /// The down directional button.
36    pub down: Button,
37    /// The left directional button.
38    pub left: Button,
39    /// The right directional button.
40    pub right: Button,
41    /// The "X" button.
42    pub x: Button,
43    /// The "Y" button.
44    pub y: Button,
45    /// The "A" button.
46    pub a: Button,
47    /// The "B" button.
48    pub b: Button,
49    /// The LCD screen
50    pub screen: Screen,
51}
52
53impl Controller {
54    /// Creates a new controller.
55    ///
56    /// # Safety
57    ///
58    /// This function is unsafe because it allows the user to create multiple
59    /// mutable references to the same controller. You likely want to implement
60    /// [`Robot::new`](crate::robot::Robot::new()) instead.
61    pub unsafe fn new(id: ControllerId) -> Self {
62        let id: bindings::controller_id_e_t = id.into();
63        Controller {
64            id,
65            left_stick: AnalogStick {
66                id,
67                x_channel: bindings::controller_analog_e_t_E_CONTROLLER_ANALOG_LEFT_X,
68                y_channel: bindings::controller_analog_e_t_E_CONTROLLER_ANALOG_LEFT_Y,
69            },
70            right_stick: AnalogStick {
71                id,
72                x_channel: bindings::controller_analog_e_t_E_CONTROLLER_ANALOG_RIGHT_X,
73                y_channel: bindings::controller_analog_e_t_E_CONTROLLER_ANALOG_RIGHT_Y,
74            },
75            l1: Button {
76                id,
77                button: bindings::controller_digital_e_t_E_CONTROLLER_DIGITAL_L1,
78            },
79            l2: Button {
80                id,
81                button: bindings::controller_digital_e_t_E_CONTROLLER_DIGITAL_L2,
82            },
83            r1: Button {
84                id,
85                button: bindings::controller_digital_e_t_E_CONTROLLER_DIGITAL_R1,
86            },
87            r2: Button {
88                id,
89                button: bindings::controller_digital_e_t_E_CONTROLLER_DIGITAL_R2,
90            },
91            up: Button {
92                id,
93                button: bindings::controller_digital_e_t_E_CONTROLLER_DIGITAL_UP,
94            },
95            down: Button {
96                id,
97                button: bindings::controller_digital_e_t_E_CONTROLLER_DIGITAL_DOWN,
98            },
99            right: Button {
100                id,
101                button: bindings::controller_digital_e_t_E_CONTROLLER_DIGITAL_RIGHT,
102            },
103            left: Button {
104                id,
105                button: bindings::controller_digital_e_t_E_CONTROLLER_DIGITAL_LEFT,
106            },
107            x: Button {
108                id,
109                button: bindings::controller_digital_e_t_E_CONTROLLER_DIGITAL_X,
110            },
111            y: Button {
112                id,
113                button: bindings::controller_digital_e_t_E_CONTROLLER_DIGITAL_Y,
114            },
115            b: Button {
116                id,
117                button: bindings::controller_digital_e_t_E_CONTROLLER_DIGITAL_B,
118            },
119            a: Button {
120                id,
121                button: bindings::controller_digital_e_t_E_CONTROLLER_DIGITAL_A,
122            },
123            screen: Screen { id, queue: None },
124        }
125    }
126
127    /// Returns false or true if the controller is connected.
128    pub fn is_connected(&self) -> Result<bool, ControllerError> {
129        match unsafe { bindings::controller_is_connected(self.id) } {
130            0 => Ok(false),
131            1 => Ok(true),
132            _ => Err(ControllerError::from_errno()),
133        }
134    }
135
136    /// Gets the battery level of the controller.
137    pub fn get_battery_level(&self) -> Result<i32, ControllerError> {
138        match unsafe { bindings::controller_get_battery_level(self.id) } {
139            bindings::PROS_ERR_ => Err(ControllerError::from_errno()),
140            x => Ok(x),
141        }
142    }
143
144    /// Gets the battery capacity of the controller.
145    pub fn get_battery_capacity(&self) -> Result<i32, ControllerError> {
146        match unsafe { bindings::controller_get_battery_capacity(self.id) } {
147            bindings::PROS_ERR_ => Err(ControllerError::from_errno()),
148            x => Ok(x),
149        }
150    }
151}
152
153impl fmt::Debug for Controller {
154    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
155        f.debug_struct("Controller").field("id", &self.id).finish()
156    }
157}
158
159impl DataSource for Controller {
160    type Data = ControllerData;
161
162    type Error = ControllerError;
163
164    fn read(&self) -> Result<Self::Data, Self::Error> {
165        Ok(ControllerData {
166            left_x: self.left_stick.get_x()?,
167            left_y: self.left_stick.get_y()?,
168            right_x: self.right_stick.get_x()?,
169            right_y: self.right_stick.get_y()?,
170            l1: self.l1.is_pressed()?,
171            l2: self.l2.is_pressed()?,
172            r1: self.r1.is_pressed()?,
173            r2: self.r2.is_pressed()?,
174            up: self.up.is_pressed()?,
175            down: self.down.is_pressed()?,
176            left: self.left.is_pressed()?,
177            right: self.right.is_pressed()?,
178            x: self.x.is_pressed()?,
179            y: self.y.is_pressed()?,
180            a: self.a.is_pressed()?,
181            b: self.b.is_pressed()?,
182            battery_level: self.get_battery_level()?,
183            battery_capacity: self.get_battery_capacity()?,
184        })
185    }
186}
187
188#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
189/// Describes data from all controller inputs.
190pub struct ControllerData {
191    /// The x-axis of the left analog stick.
192    pub left_x: i8,
193    /// The y-axis of the left analog stick.
194    pub left_y: i8,
195    /// The x-axis of the right analog stick.
196    pub right_x: i8,
197    /// The y-axis of the right analog stick.
198    pub right_y: i8,
199    /// The top-left shoulder button.
200    pub l1: bool,
201    /// The bottom-left shoulder button.
202    pub l2: bool,
203    /// The top-right shoulder button.
204    pub r1: bool,
205    /// The bottom-right shoulder button.
206    pub r2: bool,
207    /// The up directional button.
208    pub up: bool,
209    /// The down directional button.
210    pub down: bool,
211    /// The left directional button.
212    pub left: bool,
213    /// The right directional button.
214    pub right: bool,
215    /// The "X" button.
216    pub x: bool,
217    /// The "Y" button.
218    pub y: bool,
219    /// The "A" button.
220    pub a: bool,
221    /// The "B" button.
222    pub b: bool,
223    /// The battery level of the controller.
224    pub battery_level: i32,
225    /// The battery capacity of the controller.
226    pub battery_capacity: i32,
227}
228
229/// Represents one of two analog sticks on a Vex controller.
230pub struct AnalogStick {
231    id: bindings::controller_id_e_t,
232    x_channel: bindings::controller_analog_e_t,
233    y_channel: bindings::controller_analog_e_t,
234}
235
236impl AnalogStick {
237    /// Reads an analog stick's x-axis. Returns a value on the range [-127,
238    /// 127] where -127 is all the way left, 0 is centered, and 127 is all the
239    /// way right. Also returns 0 if controller is not connected.
240    pub fn get_x(&self) -> Result<i8, ControllerError> {
241        self.get_channel(self.x_channel)
242    }
243
244    /// Reads an analog stick's y-axis. Returns a value on the range [-127,
245    /// 127] where -127 is all the way down, 0 is centered, and 127 is all the
246    /// way up. Also returns 0 if controller is not connected.
247    pub fn get_y(&self) -> Result<i8, ControllerError> {
248        self.get_channel(self.y_channel)
249    }
250
251    fn get_channel(&self, channel: bindings::controller_analog_e_t) -> Result<i8, ControllerError> {
252        match unsafe { bindings::controller_get_analog(self.id, channel) } {
253            bindings::PROS_ERR_ => Err(ControllerError::from_errno()),
254            x => match x.try_into() {
255                Ok(converted_x) => Ok(converted_x),
256                Err(_) => {
257                    panic!(
258                        "bindings::controller_get_analog returned unexpected value: {}",
259                        x
260                    )
261                }
262            },
263        }
264    }
265}
266
267/// Represents a button on a Vex controller.
268pub struct Button {
269    id: bindings::controller_id_e_t,
270    button: bindings::controller_digital_e_t,
271}
272
273impl Button {
274    /// Checks if a given button is pressed. Returns false if the controller is
275    /// not connected.
276    pub fn is_pressed(&self) -> Result<bool, ControllerError> {
277        match unsafe { bindings::controller_get_digital(self.id, self.button) } {
278            0 => Ok(false),
279            1 => Ok(true),
280            _ => Err(ControllerError::from_errno()),
281        }
282    }
283}
284
285/// Represents the screen on a Vex controller
286pub struct Screen {
287    id: bindings::controller_id_e_t,
288    queue: Option<SendQueue<ScreenCommand>>,
289}
290
291impl Screen {
292    /// Clears all of the lines of the controller screen
293    pub fn clear(&mut self) {
294        self.command(ScreenCommand::Clear);
295    }
296
297    /// Clears an individual line of the controller screen. Lines range from 0
298    /// to 2
299    pub fn clear_line(&mut self, line: u8) {
300        if line > 2 {
301            return;
302        }
303        self.command(ScreenCommand::ClearLine(line));
304    }
305
306    /// Prints text to the controller LCD screen. Lines range from 0 to 2.
307    /// Columns range from 0 to 18
308    pub fn print(&mut self, line: u8, column: u8, str: &str) {
309        if line > 2 || column > 18 {
310            return;
311        }
312        let mut chars: [libc::c_char; 19] = Default::default();
313        copy(&mut chars, str.as_bytes());
314        self.command(ScreenCommand::Print {
315            chars,
316            line,
317            column,
318            length: str.as_bytes().len() as u8,
319        });
320    }
321
322    /// Rumble the controller. Rumble pattern is a string consisting of the
323    /// characters ‘.’, ‘-’, and ‘ ‘, where dots are short rumbles, dashes are
324    /// long rumbles, and spaces are pauses; all other characters are ignored.
325    /// Maximum supported length is 8 characters.
326    pub fn rumble(&mut self, rumble_pattern: &str) {
327        let mut pattern: [libc::c_char; 8] = Default::default();
328        let mut i = 0;
329        for c in rumble_pattern.chars() {
330            match c {
331                '.' | '-' | '_' => {
332                    pattern[i] = c as libc::c_char;
333                    i += 1;
334                }
335                _ => {}
336            }
337            if i >= pattern.len() {
338                break;
339            }
340        }
341        self.command(ScreenCommand::Rumble(pattern));
342    }
343
344    fn command(&mut self, cmd: ScreenCommand) {
345        self.queue().send(cmd);
346    }
347
348    fn queue(&mut self) -> &mut SendQueue<ScreenCommand> {
349        self.queue.get_or_insert_with(|| {
350            let name = match self.id {
351                bindings::controller_id_e_t_E_CONTROLLER_MASTER => "controller-screen-master",
352                bindings::controller_id_e_t_E_CONTROLLER_PARTNER => "controller-screen-partner",
353                _ => "",
354            };
355            let id = self.id;
356            let (send, recv) = queue(VecDeque::<ScreenCommand>::new());
357            Task::spawn_ext(
358                name,
359                bindings::TASK_PRIORITY_MAX,
360                bindings::TASK_STACK_DEPTH_DEFAULT as u16,
361                move || {
362                    let mut delay_target = None;
363                    let mut offset = 0usize;
364                    let mut clear = false;
365                    let mut buffer = [ScreenRow::default(); 3];
366                    let mut rumble: Option<[libc::c_char; 9]> = None;
367                    'main: loop {
368                        let command: Option<ScreenCommand> = select! {
369                            cmd = recv.select() => Some(cmd),
370                            _ = delay_until(t); Some(t) = delay_target => None,
371                        };
372                        if let Some(cmd) = command {
373                            match cmd {
374                                ScreenCommand::Clear => {
375                                    offset = 0;
376                                    clear = true;
377                                    buffer = Default::default();
378                                }
379                                ScreenCommand::ClearLine(line) => {
380                                    let row = &mut buffer[line as usize];
381                                    *row = ScreenRow::default();
382                                    row.needs_clear = true;
383                                }
384                                ScreenCommand::Print {
385                                    chars,
386                                    line,
387                                    column,
388                                    length,
389                                } => {
390                                    let row = &mut buffer[line as usize];
391                                    copy(
392                                        &mut row.chars[column as usize..],
393                                        &chars[..length as usize],
394                                    );
395                                    row.dirty = true;
396                                }
397                                ScreenCommand::Rumble(pattern) => {
398                                    let mut buf: [libc::c_char; 9] = Default::default();
399                                    copy(&mut buf, &pattern);
400                                    rumble = Some(buf);
401                                }
402                                ScreenCommand::Stop => break,
403                            }
404                        }
405                        if let Some(pattern) = rumble {
406                            match unsafe { bindings::controller_rumble(id, pattern.as_ptr()) } {
407                                1 => {
408                                    delay_target = Some(time_since_start() + SCREEN_SUCCESS_DELAY);
409                                    rumble = None;
410                                }
411                                _ => {
412                                    delay_target = Some(time_since_start() + SCREEN_FAILURE_DELAY);
413                                    Self::print_error()
414                                }
415                            }
416                        } else if clear {
417                            match unsafe { bindings::controller_clear(id) } {
418                                1 => {
419                                    delay_target = Some(time_since_start() + SCREEN_SUCCESS_DELAY);
420                                    clear = false;
421                                }
422                                _ => {
423                                    delay_target = Some(time_since_start() + SCREEN_FAILURE_DELAY);
424                                    Self::print_error()
425                                }
426                            }
427                        } else {
428                            for i in 0..3 {
429                                let index = (offset + i) % buffer.len();
430                                let row = &mut buffer[index];
431                                if row.needs_clear {
432                                    match unsafe {
433                                        bindings::controller_clear_line(id, index as u8)
434                                    } {
435                                        1 => {
436                                            delay_target =
437                                                Some(time_since_start() + SCREEN_SUCCESS_DELAY);
438                                            row.needs_clear = false;
439                                        }
440                                        _ => {
441                                            delay_target =
442                                                Some(time_since_start() + SCREEN_FAILURE_DELAY);
443                                            Self::print_error()
444                                        }
445                                    }
446                                } else if row.dirty {
447                                    match unsafe {
448                                        bindings::controller_set_text(
449                                            id,
450                                            index as u8,
451                                            0,
452                                            row.chars.as_ptr(),
453                                        )
454                                    } {
455                                        1 => {
456                                            delay_target =
457                                                Some(time_since_start() + SCREEN_SUCCESS_DELAY);
458                                            row.dirty = false;
459                                        }
460                                        _ => {
461                                            delay_target =
462                                                Some(time_since_start() + SCREEN_FAILURE_DELAY);
463                                            Self::print_error()
464                                        }
465                                    }
466                                } else {
467                                    continue;
468                                }
469                                offset = i + 1;
470                                continue 'main;
471                            }
472                            // No updates were made; delay indefinitely until next command.
473                            delay_target = None;
474                        }
475                    }
476                },
477            )
478            .unwrap();
479            send
480        })
481    }
482
483    fn print_error() {
484        if get_errno() != libc::EAGAIN {
485            eprintln!("{:?}", ControllerError::from_errno());
486        }
487    }
488}
489
490impl Drop for Screen {
491    fn drop(&mut self) {
492        if self.queue.is_some() {
493            self.command(ScreenCommand::Stop);
494        }
495    }
496}
497
498#[derive(Copy, Clone)]
499struct ScreenRow {
500    chars: [libc::c_char; 20],
501    dirty: bool,
502    needs_clear: bool,
503}
504
505impl Default for ScreenRow {
506    fn default() -> Self {
507        // All spaces except last.
508        let mut chars = [0x20; 20];
509        chars[19] = 0;
510        Self {
511            chars,
512            dirty: Default::default(),
513            needs_clear: Default::default(),
514        }
515    }
516}
517
518#[derive(Debug)]
519enum ScreenCommand {
520    Clear,
521    ClearLine(u8),
522    Print {
523        chars: [libc::c_char; 19],
524        line: u8,
525        column: u8,
526        length: u8,
527    },
528    Rumble([libc::c_char; 8]),
529    Stop,
530}
531
532/// Represents the two types of controller.
533pub enum ControllerId {
534    /// The primary controller.
535    Master,
536    /// The tethered/partner controller.
537    Partner,
538}
539
540impl From<ControllerId> for bindings::controller_id_e_t {
541    fn from(id: ControllerId) -> Self {
542        match id {
543            ControllerId::Master => bindings::controller_id_e_t_E_CONTROLLER_MASTER,
544            ControllerId::Partner => bindings::controller_id_e_t_E_CONTROLLER_PARTNER,
545        }
546    }
547}
548
549/// Represents possible error states for a controller.
550#[derive(Debug)]
551pub enum ControllerError {
552    /// Controller ID does not exist.
553    InvalidController,
554    /// Another resource is currently trying to access the controller port.
555    ControllerBusy,
556    /// Unknown Errno.
557    Unknown(i32),
558}
559
560impl ControllerError {
561    fn from_errno() -> Self {
562        match { get_errno() } {
563            libc::EINVAL => Self::InvalidController,
564            libc::EACCES => Self::ControllerBusy,
565            x => Self::Unknown(x),
566        }
567    }
568}
569
570impl From<ControllerError> for Error {
571    fn from(err: ControllerError) -> Self {
572        match err {
573            ControllerError::InvalidController => Error::Custom("invalid controller id".into()),
574            ControllerError::ControllerBusy => Error::Custom("controller is busy".into()),
575            ControllerError::Unknown(n) => Error::System(n),
576        }
577    }
578}
vex_rt/controller.rs

vex_rt/
controller.rs
