From 025b05e3de685bfc6409ed5fabe8c38a972fac52 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Sun, 9 Feb 2025 13:37:52 -0500
Subject: [PATCH] implemented brownout on northbridge

this is unsound in that only the most recent command gets modulated by
the bus voltage. In practice this should be fine as commands should come
over the pipe regularly in situations that cause a brownout
---
 northbridge/src/main.rs | 67 +++++++++++++++++++++++++++++++++++------
 1 file changed, 58 insertions(+), 9 deletions(-)

diff --git a/northbridge/src/main.rs b/northbridge/src/main.rs
index ff81a42..a09564a 100644
--- a/northbridge/src/main.rs
+++ b/northbridge/src/main.rs
@@ -1,17 +1,23 @@
 #![feature(async_closure)]
 
-use std::time::Duration;
+use std::{sync::Arc, time::Duration};
 
 use anyhow::{Context, Result};
 use common::{Command, Response, SensorData, BAUDRATE};
 use framed_codec::FramedCodec;
 use futures::{SinkExt, StreamExt};
-use tokio::{io::{AsyncReadExt, AsyncWriteExt}, net::{TcpListener, TcpSocket}, sync::{broadcast::{self, error::RecvError, Receiver}, watch::{self, Sender}}, task::JoinHandle, time::{error::Elapsed, timeout}};
+use nalgebra::{SimdPartialOrd, SimdValue, Vector3};
+use tokio::{io::{AsyncReadExt, AsyncWriteExt}, net::{TcpListener, TcpSocket}, sync::{broadcast::{self, error::RecvError, Receiver}, watch::{self, Sender}, RwLock}, task::JoinHandle, time::{error::Elapsed, timeout}};
 use tokio_serial::SerialPortBuilderExt;
 use tokio_util::codec::Framed;
 
 mod framed_codec;
 
+#[derive(Default)]
+struct RobotState {
+    bus_voltage: f32,
+}
+
 #[tokio::main]
 async fn main() -> Result<()> {
     let mut serial = tokio_serial::new("/dev/ttyAMA0", BAUDRATE).open_native_async()?;
@@ -24,12 +30,12 @@ async fn main() -> Result<()> {
 
     let (send, commands) = watch::channel(Command::Stop);
 
-    //let mut print_telem = sensor_data.resubscribe();
-    //let _: JoinHandle<Result<()>> = tokio::spawn(async move {
-    //    loop {
-    //        println!("sensor {:?}", print_telem.recv().await?);
-    //    }
-    //});
+    let state = Arc::new(RwLock::new(RobotState {
+        bus_voltage: f32::MAX,
+        ..Default::default()
+    }));
+
+    tokio::spawn(update_telem(state.clone(), sensor_data.resubscribe()));
 
     let control_telem = sensor_data.resubscribe();
     tokio::spawn(async move {
@@ -65,8 +71,13 @@ async fn main() -> Result<()> {
         if let Some(data) = data.sensor_data {
             let _ = sensor_sender.send(data);
         }
+
+        let voltage = state.read().await.bus_voltage;
+
+        // 100% effort at 9.5v, 0% effort at 7.5v
+        let brownout = ((voltage-7.5)/2.0).clamp(0., 1.);
         
-        write.send(commands.borrow().clone()).await?;
+        write.send(commands.borrow().clone().brownout(brownout)).await?;
         write.flush().await?;
     }
 }
@@ -110,3 +121,41 @@ async fn control(sender: Sender<Command>, mut telem: Receiver<SensorData>) -> Re
         sender.send(cmd)?;
     }
 }
+
+async fn update_telem(state: Arc<RwLock<RobotState>>, mut telem: Receiver<SensorData>) -> Result<()> {
+    loop {
+        let telem = telem.recv().await?;
+        println!("sensor {telem:?}");
+
+        match telem {
+            SensorData::BusVoltage(voltage) => {
+                state.write().await.bus_voltage = voltage;
+            }
+            _ => {}
+        }
+
+    }
+}
+
+trait Brownout {
+    /// limit exertion to factor of full output (0 to 1)
+    fn brownout(self, factor: f32) -> Self;
+}
+
+impl Brownout for Command {
+    fn brownout(self, factor: f32) -> Self {
+        // nothing is preserved
+        match self {
+            Self::Twist(fwd, rot) => Self::Twist(
+                fwd.clamp(-factor, factor), 
+                rot.clamp(-factor, factor)
+            ),
+            Self::SetLed(rgb) => {
+                Self::SetLed(rgb.simd_min(Vector3::repeat((factor * u8::MAX as f32) as u8)))
+            }
+            other => other,
+        }
+    }
+}
+
+