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

@@ -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,
+        }
+    }
+}
+
+