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> = 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, mut telem: Receiver) -> Re sender.send(cmd)?; } } + +async fn update_telem(state: Arc>, mut telem: Receiver) -> 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, + } + } +} + +