underglow

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]]

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"

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;
     }
 }

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);
         }

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<u8>) {
+    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);
+}