diff --git a/src/constants.rs b/src/constants.rs index 8748c87..4dd9cc1 100644 --- a/src/constants.rs +++ b/src/constants.rs @@ -30,6 +30,11 @@ pub mod robotmap { pub const RAISE: i32 = 0; pub const GRAB: i32 = 1; } + + pub mod led { + pub const PORT: i32 = 1; + pub const COUNT: i32 = 0; + } } // TODO: get 2025 field dimensions diff --git a/src/subsystems/led.rs b/src/subsystems/led.rs new file mode 100644 index 0000000..ba0bb96 --- /dev/null +++ b/src/subsystems/led.rs @@ -0,0 +1,74 @@ +use std::cell::RefCell; +use std::cmp::PartialEq; +use std::rc::Rc; +use std::sync::{Arc, Mutex}; +use std::time::Duration; +use crate::constants::robotmap; +use ::frcrs::led::Led; +use tokio::task::{AbortHandle, spawn_local}; +use tokio::time::sleep; + +#[derive(Clone, PartialEq)] +pub enum LedStatus { + Disabled +} + +#[derive(Clone)] +pub struct LedSubsystem { + led: Led, + handle: Option, + current_state: LedStatus, +} + +impl LedSubsystem { + pub fn init() -> Self { + let led = Led::new(robotmap::led::PORT, robotmap::led::COUNT); + + Self { + led, + handle: None, + current_state: LedStatus::Disabled, + } + } + + pub fn set_state(&mut self, state: LedStatus) { + if state == self.current_state { + return; + } else { + self.current_state = state; + } + + if let Some(handle) = self.handle.take() { + handle.abort(); + } + + let mut led_clone = self.led.clone(); + + let future = match state { + LedStatus::Disabled => { + async move { + LedSubsystem::disabled(&mut led_clone).await; + } + } + }; + + let handle = spawn_local(future).abort_handle(); + + self.handle = Some(handle); + } + + async fn disabled(led: &mut Led) { + loop { + for i in (0..=robotmap::led::COUNT).step_by(2) { + led.set_rgb(i, 255, 0, 0); + } + + for i in (1..=robotmap::led::COUNT).step_by(2) { + led.set_rgb(i, 0, 0, 0); + } + + led.set_data(); + sleep(Duration::from_secs_f64(0.5)).await; + } + } +} diff --git a/src/subsystems/mod.rs b/src/subsystems/mod.rs index 4062420..f69dbdf 100644 --- a/src/subsystems/mod.rs +++ b/src/subsystems/mod.rs @@ -2,6 +2,7 @@ mod climber; mod drivetrain; mod elevator; mod indexer; +mod led; mod vision; pub use climber::*;