From 0a2f22a4f1e4c976f7204e9b5b8560a74ce1fcdb Mon Sep 17 00:00:00 2001 From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com> Date: Sun, 9 Feb 2025 18:44:34 -0500 Subject: [PATCH] underglow --- interface/Cargo.lock | 81 +++++++++++++++++++++++++++++++++++++---- interface/Cargo.toml | 1 + interface/src/main.rs | 24 +++++++++++- northbridge/src/main.rs | 3 -- southbridge/src/main.rs | 40 +++++++++++++++++++- 5 files changed, 136 insertions(+), 13 deletions(-) diff --git a/interface/Cargo.lock b/interface/Cargo.lock index 70804d3..8f88426 100644 --- a/interface/Cargo.lock +++ b/interface/Cargo.lock @@ -74,6 +74,12 @@ version = "3.17.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1628fb46dfa0b37568d12e5edd512553eccf6a22a78e8bde00bb4aed84d5bdbf" +[[package]] +name = "bytemuck" +version = "1.21.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ef657dfab802224e671f5818e9a4935f9b1957ed18e58292690cc39e7a4092a3" + [[package]] name = "byteorder" version = "1.5.0" @@ -208,7 +214,7 @@ checksum = "162ee34ebcb7c64a8abebc059ce0fee27c2262618d7b60ed8faf72fef13c3650" dependencies = [ "proc-macro2", "quote", - "syn", + "syn 2.0.98", ] [[package]] @@ -332,6 +338,7 @@ dependencies = [ "common", "futures", "gilrs", + "nalgebra", "postcard", "serde", "tokio", @@ -398,6 +405,16 @@ dependencies = [ "libc", ] +[[package]] +name = "matrixmultiply" +version = "0.3.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9380b911e3e96d10c1f415da0876389aaf1b56759054eeb0de7df940c456ba1a" +dependencies = [ + "autocfg", + "rawpointer", +] + [[package]] name = "memchr" version = "2.7.4" @@ -431,6 +448,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "20bd243ab3dbb395b39ee730402d2e5405e448c75133ec49cc977762c4cba3d1" dependencies = [ "approx", + "matrixmultiply", + "nalgebra-macros", "num-complex", "num-rational", "num-traits", @@ -439,6 +458,17 @@ dependencies = [ "typenum", ] +[[package]] +name = "nalgebra-macros" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "01fcc0b8149b4632adc89ac3b7b31a12fb6099a0317a4eb2ebff574ef7de7218" +dependencies = [ + "proc-macro2", + "quote", + "syn 1.0.109", +] + [[package]] name = "nix" version = "0.29.0" @@ -582,6 +612,12 @@ dependencies = [ "proc-macro2", ] +[[package]] +name = "rawpointer" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" + [[package]] name = "redox_syscall" version = "0.5.8" @@ -612,6 +648,15 @@ version = "1.0.19" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f7c45b9784283f1b2e7fb61b42047c2fd678ef0960d4f6f1eba131594cc369d4" +[[package]] +name = "safe_arch" +version = "0.7.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96b02de82ddbe1b636e6170c21be622223aea188ef2e139be0a5b219ec215323" +dependencies = [ + "bytemuck", +] + [[package]] name = "scopeguard" version = "1.2.0" @@ -641,7 +686,7 @@ checksum = "5a9bf7cf98d04a2b28aead066b7496853d4779c9cc183c440dbac457641e19a0" dependencies = [ "proc-macro2", "quote", - "syn", + "syn 2.0.98", ] [[package]] @@ -663,6 +708,7 @@ dependencies = [ "num-complex", "num-traits", "paste", + "wide", ] [[package]] @@ -705,6 +751,17 @@ version = "1.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" +[[package]] +name = "syn" +version = "1.0.109" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + [[package]] name = "syn" version = "2.0.98" @@ -742,7 +799,7 @@ checksum = "6e06d43f1345a3bcd39f6a56dbb7dcab2ba47e68e8ac134855e7e2bdbaf8cab8" dependencies = [ "proc-macro2", "quote", - "syn", + "syn 2.0.98", ] [[package]] @@ -797,7 +854,7 @@ dependencies = [ "log", "proc-macro2", "quote", - "syn", + "syn 2.0.98", "wasm-bindgen-shared", ] @@ -819,7 +876,7 @@ checksum = "8ae87ea40c9f689fc23f209965b6fb8a99ad69aeeb0231408be24920604395de" dependencies = [ "proc-macro2", "quote", - "syn", + "syn 2.0.98", "wasm-bindgen-backend", "wasm-bindgen-shared", ] @@ -843,6 +900,16 @@ dependencies = [ "wasm-bindgen", ] +[[package]] +name = "wide" +version = "0.7.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41b5576b9a81633f3e8df296ce0063042a73507636cbe956c61133dd7034ab22" +dependencies = [ + "bytemuck", + "safe_arch", +] + [[package]] name = "windows" version = "0.59.0" @@ -874,7 +941,7 @@ checksum = "83577b051e2f49a058c308f17f273b570a6a758386fc291b5f6a934dd84e48c1" dependencies = [ "proc-macro2", "quote", - "syn", + "syn 2.0.98", ] [[package]] @@ -885,7 +952,7 @@ checksum = "cb26fd936d991781ea39e87c3a27285081e3c0da5ca0fcbc02d368cc6f52ff01" dependencies = [ "proc-macro2", "quote", - "syn", + "syn 2.0.98", ] [[package]] diff --git a/interface/Cargo.toml b/interface/Cargo.toml index 9fd029a..1045631 100644 --- a/interface/Cargo.toml +++ b/interface/Cargo.toml @@ -11,3 +11,4 @@ gilrs = "0.11.0" futures = "0.3.31" serde = { version = "1.0.217", features = ["alloc"] } postcard = { version = "1.0.0", features = ["alloc", "use-std"] } +nalgebra = "0.31.2" diff --git a/interface/src/main.rs b/interface/src/main.rs index 2ba68cd..eec7eed 100644 --- a/interface/src/main.rs +++ b/interface/src/main.rs @@ -3,6 +3,7 @@ use std::time::Duration; use anyhow::Context; use common::{Command, SensorData}; use gilrs::{Axis, Event, Gilrs}; +use nalgebra::Vector3; use tokio::{io::{AsyncReadExt, AsyncWriteExt, BufWriter}, net::{tcp::OwnedReadHalf, TcpStream}, task::JoinHandle, time::{sleep, Instant}}; #[tokio::main] @@ -13,6 +14,8 @@ async fn main() -> anyhow::Result<()> { let (_, gamepad) = gamepads.gamepads().nth(0).context("no gamepads")?; let ly = gamepad.axis_code(Axis::LeftStickY).context("no left joystick")?; let rx = gamepad.axis_code(Axis::RightStickX).context("no right joystick")?; + let lb = gamepad.button_code(gilrs::Button::LeftTrigger2).context("no lb")?; + let a = gamepad.button_code(gilrs::Button::South).context("no a")?; let mut robot = TcpStream::connect("pelican.dyn.wpi.edu:3322").await?; robot.set_nodelay(true)?; @@ -43,10 +46,18 @@ async fn main() -> anyhow::Result<()> { let mut ly= values.axis_data(ly).map(|d| d.value()).unwrap_or(0.0); let mut rx= values.axis_data(rx).map(|d| d.value()).unwrap_or(0.0); + let boost = values.button_data(lb).map(|b| b.is_pressed()).unwrap_or(false); + + let a = values.button_data(a).map(|b| b.is_pressed()).unwrap_or(false); + for axis in [&mut rx, &mut ly] { if axis.abs() < 0.05 { *axis = 0.0; } + + if !boost { + *axis *= 0.3; + } } if !gamepad.is_connected() { @@ -57,10 +68,11 @@ async fn main() -> anyhow::Result<()> { let cmd = Command::Twist(ly, rx); let encoded = postcard::to_stdvec(&cmd)?; - robot_controller.write_u32(encoded.len() as u32).await?; robot_controller.write_all(&encoded).await?; + + if let Err(e) = robot_controller.flush().await { println!("flush fail: {e}, reconnecting"); robot = TcpStream::connect("pelican.dyn.wpi.edu:3322").await?; @@ -72,6 +84,16 @@ async fn main() -> anyhow::Result<()> { robot_controller = BufWriter::new(robot); }; + + let black = Vector3::repeat(0u8); + let green = Vector3::new(100u8, 0, 0); + let cmd = Command::SetLed(if a {green} else {black}); + let encoded = postcard::to_stdvec(&cmd)?; + robot_controller.write_u32(encoded.len() as u32).await?; + robot_controller.write_all(&encoded).await?; + sleep(Duration::from_micros(350)).await; + robot_controller.flush().await?; + sleep(Duration::from_micros(3500)).await; } } diff --git a/northbridge/src/main.rs b/northbridge/src/main.rs index a09564a..357b8d4 100644 --- a/northbridge/src/main.rs +++ b/northbridge/src/main.rs @@ -65,9 +65,6 @@ async fn main() -> Result<()> { write.send(Command::Enable).await?; } - write.send(Command::Enable).await?; - write.flush().await?; - if let Some(data) = data.sensor_data { let _ = sensor_sender.send(data); } diff --git a/southbridge/src/main.rs b/southbridge/src/main.rs index 13ce640..54ff24e 100644 --- a/southbridge/src/main.rs +++ b/southbridge/src/main.rs @@ -10,7 +10,7 @@ use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channe use embassy_time::{with_deadline, with_timeout, Duration, Instant, Timer}; use embedded_io_async::{BufRead, Read, Write}; use log::{error, info, trace, warn}; -use nalgebra::clamp; +use nalgebra::{clamp, Vector3}; use portable_atomic::AtomicBool; use static_cell::{ConstStaticCell, StaticCell}; @@ -58,6 +58,22 @@ async fn main(spawner: Spawner) { let stopped = drive_conf.clone(); let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE0, p.PIN_16, p.PIN_17, stopped.clone()); + let disabled_orange = Vector3::new(254, 100, 0); + + let mut light_conf_rg: pwm::Config = Default::default(); + light_conf_rg.divider = 1.into(); + light_conf_rg.top = 255; + light_conf_rg.compare_b = 0; + light_conf_rg.compare_a = 0; + let mut light_pwm_rg = Pwm::new_output_ab(p.PWM_SLICE7, p.PIN_14, p.PIN_15, light_conf_rg); + + let mut light_conf_b: pwm::Config = Default::default(); + light_conf_b.divider = 1.into(); + light_conf_b.top = 255; + light_conf_b.compare_b = 0; + let mut light_pwm_b = Pwm::new_output_b(p.PWM_SLICE6, p.PIN_13, light_conf_b); + set_underglow(&mut light_pwm_rg, &mut light_pwm_b, disabled_orange.clone()); + Timer::after_secs(13).await; // pi 0 serial ports act strange during the boot process spawner.spawn(bus_voltage_monitor(p.ADC, p.PIN_28, p.ADC_TEMP_SENSOR)).unwrap(); @@ -94,6 +110,7 @@ async fn main(spawner: Spawner) { let Ok(command) = with_deadline(enable_watchdog + enable_watchdog_time, COMMANDS.receive()).await else { warn!("no message received"); enabled.store(false, Ordering::Release); + set_underglow(&mut light_pwm_rg, &mut light_pwm_b, disabled_orange.clone()); continue; }; command @@ -118,15 +135,19 @@ async fn main(spawner: Spawner) { Command::Enable => { enabled.store(true, Ordering::Release); enable_watchdog = Instant::now(); + set_underglow(&mut light_pwm_rg, &mut light_pwm_b, Vector3::new(0, 154, 0)); }, Command::Disable => { enabled.store(false, Ordering::Release); + set_underglow(&mut light_pwm_rg, &mut light_pwm_b, disabled_orange.clone()); drive_pwm.set_config(&stopped); }, Command::FeedWatchdog => { enable_watchdog = Instant::now(); } - c => { error!("{c:?} unimplemented") } + Command::SetLed(value) => { + set_underglow(&mut light_pwm_rg, &mut light_pwm_b, value); + } }; info!("Blink"); @@ -217,3 +238,18 @@ async fn bus_voltage_monitor(adc: ADC, bus: PIN_28, temp: ADC_TEMP_SENSOR) { Timer::after_millis(3).await; } } + +fn set_underglow(rg: &mut Pwm, b: &mut Pwm, color: Vector3) { + let mut light_conf_rg: pwm::Config = Default::default(); + light_conf_rg.divider = 1.into(); + light_conf_rg.top = 255; + light_conf_rg.compare_b = color[1] as u16; + light_conf_rg.compare_a = color[0] as u16; + rg.set_config(&light_conf_rg); + + let mut light_conf_b: pwm::Config = Default::default(); + light_conf_b.divider = 1.into(); + light_conf_b.top = 255; + light_conf_b.compare_b = color[2] as u16; + b.set_config(&light_conf_b); +}