diff --git a/common/src/lib.rs b/common/src/lib.rs index f13f358..41635ab 100644 --- a/common/src/lib.rs +++ b/common/src/lib.rs @@ -1,7 +1,7 @@ //! Data types shared between the northbridge and southbridge for serial communications #![no_std] -use nalgebra::Vector3; +use nalgebra::{SimdPartialOrd, Vector3}; use serde::{Deserialize, Serialize}; pub const BAUDRATE: u32 = 115200; @@ -20,9 +20,34 @@ pub enum Command { Disable, } +#[derive(Serialize, Deserialize, Debug, Clone)] +pub struct RobotState { + /// forward. clockwise + pub drive: (f32,f32), + /// RGB led brightness + pub underglow: Vector3, +} + +impl RobotState { + /// limit exertion to factor of full output (0 to 1) + pub fn brownout(self, factor: f32) -> Self { + Self { + drive: ( + self.drive.0.clamp(-factor, factor), + self.drive.1.clamp(-factor, factor), + ), + underglow: self.underglow.simd_min(Vector3::repeat((factor * u8::MAX as f32) as u8)) + } + } + + /// Safe command to send in uncertian states + pub fn stopped() -> Self { + Self { drive: (0.0, 0.0), underglow: Vector3::new(100, 0, 0) } + } +} + #[derive(Serialize, Deserialize, Debug)] pub struct Response { - pub enabled: bool, pub sensor_data: Option, pub uptime_micros: u64, } diff --git a/interface/src/main.rs b/interface/src/main.rs index 7304de9..23d4c58 100644 --- a/interface/src/main.rs +++ b/interface/src/main.rs @@ -1,7 +1,7 @@ use std::time::Duration; use anyhow::Context; -use common::{Command, SensorData}; +use common::{RobotState, SensorData}; use gilrs::{Axis, Event, Gilrs}; use nalgebra::Vector3; use tokio::{io::{AsyncReadExt, AsyncWriteExt, BufWriter}, net::{tcp::OwnedReadHalf, TcpStream}, task::JoinHandle, time::{sleep, Instant}}; @@ -18,7 +18,7 @@ async fn main() -> anyhow::Result<()> { let a = gamepad.button_code(gilrs::Button::South).context("no a")?; let mut last_a = false; - let server = "130.215.211.79:3322"; + let server = "pelican.dyn.wpi.edu:3322"; let mut robot = TcpStream::connect(server).await?; robot.set_nodelay(true)?; @@ -66,26 +66,20 @@ async fn main() -> anyhow::Result<()> { ly = 0.0; rx = 0.0; } + let black = Vector3::repeat(0u8); + let green = Vector3::new(254u8, 0, 254); + let color = if a {green} else {black}; - let cmd = Command::Twist(ly, rx); - println!("twist"); + let cmd = RobotState { + drive: (ly, rx), + underglow: color, + + }; let encoded = postcard::to_stdvec(&cmd)?; robot_controller.write_u32(encoded.len() as u32).await?; robot_controller.write_all(&encoded).await?; - if a != last_a { - let black = Vector3::repeat(0u8); - let green = Vector3::new(254u8, 0, 254); - 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?; - println!("led"); - } - last_a = a; - - if let Err(e) = robot_controller.flush().await { println!("flush fail: {e}, reconnecting"); robot = TcpStream::connect(server).await?; @@ -98,7 +92,7 @@ async fn main() -> anyhow::Result<()> { robot_controller = BufWriter::new(robot); }; - sleep(Duration::from_millis(65)).await; + sleep(Duration::from_millis(25)).await; } } diff --git a/northbridge/src/framed_codec.rs b/northbridge/src/framed_codec.rs index 5b0312b..fcc7f34 100644 --- a/northbridge/src/framed_codec.rs +++ b/northbridge/src/framed_codec.rs @@ -1,7 +1,7 @@ use std::result; use anyhow::{anyhow, Ok}; -use common::{Command, Response}; +use common::{Response, RobotState}; use framed::{BoxPayload, FRAME_END_SYMBOL}; use tokio_util::{bytes::BytesMut, codec::{Decoder, Encoder}}; @@ -55,10 +55,10 @@ impl Encoder> for FramedCodec { } } -impl Encoder for FramedCodec { +impl Encoder for FramedCodec { type Error = anyhow::Error; - fn encode(&mut self, item: Command, dst: &mut BytesMut) -> Result<(), Self::Error> { + fn encode(&mut self, item: RobotState, dst: &mut BytesMut) -> Result<(), Self::Error> { let data = postcard::to_stdvec(&item)?; Ok(self.encode(data, dst).map_err(|e| anyhow!("encode fail: {e:?}"))?) } diff --git a/northbridge/src/main.rs b/northbridge/src/main.rs index 4c3a791..f0d4125 100644 --- a/northbridge/src/main.rs +++ b/northbridge/src/main.rs @@ -3,19 +3,19 @@ use std::{sync::Arc, time::Duration}; use anyhow::{Context, Result}; -use common::{Command, Response, SensorData, BAUDRATE}; +use common::{Response, RobotState, SensorData, BAUDRATE}; use crossbeam::queue::ArrayQueue; use framed_codec::FramedCodec; -use futures::{SinkExt, StreamExt, TryStreamExt}; +use futures::{FutureExt, SinkExt, StreamExt, TryStreamExt}; use nalgebra::{SimdPartialOrd, SimdValue, Vector3}; -use tokio::{io::{AsyncReadExt, AsyncWriteExt}, net::{TcpListener, TcpSocket}, sync::{broadcast::{self, error::RecvError, Sender, Receiver}, watch::{self }, RwLock}, task::JoinHandle, time::{error::Elapsed, timeout}}; +use tokio::{io::{AsyncReadExt, AsyncWriteExt}, net::{TcpListener, TcpSocket}, sync::{broadcast::{self, error::RecvError, Receiver, Sender}, watch::{self, channel }, RwLock}, task::JoinHandle, time::{error::Elapsed, timeout}}; use tokio_serial::SerialPortBuilderExt; use tokio_util::codec::Framed; mod framed_codec; #[derive(Default)] -struct RobotState { +struct RobotInfo { bus_voltage: f32, } @@ -29,9 +29,13 @@ async fn main() -> Result<()> { let (mut write, mut read) = Framed::new(serial, FramedCodec::new()).split(); - let commands = Arc::new(ArrayQueue::new(5)); + loop { + write.send(RobotState::stopped()).await?; + } - let state = Arc::new(RwLock::new(RobotState { + let (sender, mut command_receiver) = channel(RobotState::stopped()); + + let state = Arc::new(RwLock::new(RobotInfo { bus_voltage: f32::MAX, ..Default::default() })); @@ -39,57 +43,34 @@ async fn main() -> Result<()> { tokio::spawn(update_telem(state.clone(), sensor_data.resubscribe())); let control_telem = sensor_data.resubscribe(); - let sender = commands.clone(); tokio::spawn(async move { loop { - let _ = sender.force_push(Command::Stop); if let Err(e) = control(sender.clone(), control_telem.resubscribe()).await { println!("controller exited: {e}"); } } }); - println!("starting"); - write.send(Command::Enable).await?; - write.flush().await?; - println!("enable"); + tokio::spawn(async move { + let telem_sender = sensor_sender.clone(); + read.for_each( async |telem| { + let Ok(telem) = telem else {return;}; + let Some(data) = telem.sensor_data else {return;}; + telem_sender.send(data).unwrap(); + }).await; + }); loop { - let Some(Ok(data)) = read.next().await else { - continue; - }; - if data.enabled { - write.send(Command::FeedWatchdog).await?; + let _ = timeout(Duration::from_millis(20), command_receiver.changed()); + let command = command_receiver.borrow(); - } else { - println!("enabled southbridge"); - write.send(Command::Enable).await?; - } + println!("sending {command:?}"); - 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.); - - let mut comand = Vec::new(); - for _ in 0..commands.len().min(2) { // while let will never exit - if let Some(command) = commands.pop() { - comand.push(command); - } - } - for command in dbg!(comand) { - write.feed(command.brownout(brownout)).await?; - write.feed(Command::FeedWatchdog).await?; - } - write.flush().await?; + write.send(command.clone()).await?; } } -async fn control(sender: Arc>, mut telem: Receiver) -> Result<()> { +async fn control(sender: watch::Sender, mut telem: Receiver) -> Result<()> { let listener = TcpListener::bind("0.0.0.0:3322").await?; let (stream, addr) = listener.accept().await?; @@ -116,7 +97,7 @@ async fn control(sender: Arc>, mut telem: Receiver { break it?; }, Err(Elapsed) => { - sender.force_push(Command::Stop); + sender.send(RobotState::stopped())?; continue; }, }; @@ -124,13 +105,13 @@ async fn control(sender: Arc>, mut telem: Receiver>, mut telem: Receiver) -> Result<()> { +async fn update_telem(state: Arc>, mut telem: Receiver) -> Result<()> { loop { let telem = telem.recv().await?; println!("sensor {telem:?}"); @@ -144,26 +125,3 @@ async fn update_telem(state: Arc>, mut telem: Receiver 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, - } - } -} - - diff --git a/southbridge/src/main.rs b/southbridge/src/main.rs index 54ff24e..e20378a 100644 --- a/southbridge/src/main.rs +++ b/southbridge/src/main.rs @@ -1,12 +1,12 @@ #![no_std] #![no_main] -use core::{panic::PanicInfo, sync::atomic::Ordering}; +use core::{panic::PanicInfo, sync::atomic::{AtomicU16, Ordering}}; -use common::{Command, Response, SensorData, BAUDRATE}; +use common::{Response, RobotState, SensorData, BAUDRATE}; use embassy_executor::Spawner; use embassy_rp::{adc::{self, Adc}, bind_interrupts, block::ImageDef, gpio::{Level, Output, Pull}, peripherals::{ADC, ADC_TEMP_SENSOR, PIN_28, UART0, UART1, USB}, pwm::{self, Pwm}, uart::{BufferedInterruptHandler, BufferedUart, BufferedUartRx, BufferedUartTx, Config}, usb::Driver}; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel, once_lock::OnceLock}; +use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel, once_lock::OnceLock, watch::Watch}; use embassy_time::{with_deadline, with_timeout, Duration, Instant, Timer}; use embedded_io_async::{BufRead, Read, Write}; use log::{error, info, trace, warn}; @@ -21,9 +21,17 @@ bind_interrupts!(struct Irqs { ADC_IRQ_FIFO => adc::InterruptHandler; }); -pub static COMMANDS: Channel = Channel::new(); +pub static COMMANDS: Watch = Watch::new(); pub static SENSOR_DATA: Channel = Channel::new(); +pub static BUS_VOLTAGE: AtomicU16 = AtomicU16::new(u16::MAX); +const BUS_ADC_TO_VOLTS: f32 = 251.6763848397; + +/// volts +fn bus_voltage() -> f32 { + BUS_VOLTAGE.load(Ordering::Acquire) as f32 / BUS_ADC_TO_VOLTS +} + #[link_section = ".start_block"] #[used] pub static IMAGE_DEF: ImageDef = ImageDef::secure_exe(); @@ -49,6 +57,7 @@ async fn main(spawner: Spawner) { let driver = Driver::new(p.USB, Irqs); spawner.spawn(logger_task(driver)).unwrap(); + info!("logger enabled"); let mut drive_conf: pwm::Config = Default::default(); drive_conf.divider = 150.into(); @@ -59,6 +68,7 @@ async fn main(spawner: Spawner) { 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 enabled_green = Vector3::new(0, 140, 0); let mut light_conf_rg: pwm::Config = Default::default(); light_conf_rg.divider = 1.into(); @@ -81,9 +91,9 @@ async fn main(spawner: Spawner) { let config = embassy_rp::i2c::Config::default(); let bus = embassy_rp::i2c::I2c::new_async(p.I2C1, p.PIN_19, p.PIN_18, Irqs, config); - static TX_BUF: ConstStaticCell<[u8; 1024]> = ConstStaticCell::new([0u8;1024]); + static TX_BUF: ConstStaticCell<[u8; 2048]> = ConstStaticCell::new([0u8;2048]); let tx_buf = TX_BUF.take(); - static RX_BUF: ConstStaticCell<[u8; 1024]> = ConstStaticCell::new([0u8;1024]); + static RX_BUF: ConstStaticCell<[u8; 2048]> = ConstStaticCell::new([0u8;2048]); let rx_buf = RX_BUF.take(); let mut uart_config = Config::default(); @@ -94,63 +104,46 @@ async fn main(spawner: Spawner) { spawner.spawn(decoder(rx)).unwrap(); - static STARTUP: OnceLock<()> = OnceLock::new(); - static ENABLED: AtomicBool = AtomicBool::new(false); - spawner.spawn(telemetry(tx, &ENABLED, &STARTUP)).unwrap(); + spawner.spawn(telemetry(tx, &ENABLED)).unwrap(); - let enabled = &ENABLED; - let mut enable_watchdog = Instant::now(); - let enable_watchdog_time = Duration::from_millis(150); + let watchdog_time = Duration::from_millis(150); info!("ready"); + let mut commands = COMMANDS.receiver().unwrap(); + loop { - let command = if enabled.load(Ordering::Acquire) { - 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 - } else { - // stop all motors + let motor_update = Timer::after_millis(3); + let (command, time) = commands.get().await; + + if time.elapsed() > watchdog_time { drive_pwm.set_config(&stopped); - info!("waiting for command"); - COMMANDS.receive().await - }; + set_underglow(&mut light_pwm_rg, &mut light_pwm_b, disabled_orange.clone()); + ENABLED.store(false, Ordering::Acquire); + commands.changed().await; + set_underglow(&mut light_pwm_rg, &mut light_pwm_b, enabled_green.clone()); + ENABLED.store(true, Ordering::Acquire); + continue; + } - STARTUP.get_or_init(||()); + ENABLED.store(true, Ordering::Acquire); - match command { - Command::Twist(forward, right) => { - drive_conf.compare_a = calc_speed(-forward - right); - drive_conf.compare_b = calc_speed(forward - right); - drive_pwm.set_config(&drive_conf); - }, - Command::Stop => { - drive_pwm.set_config(&stopped); - }, - 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(); - } - Command::SetLed(value) => { - set_underglow(&mut light_pwm_rg, &mut light_pwm_b, value); - } - }; - info!("Blink"); + // 100% effort at 9.5v, 0% effort at 7.5v + let brownout = ((bus_voltage()-7.5)/2.0).clamp(0., 1.); + let command = command.brownout(brownout); + + // drive motors + let (forward, right) = command.drive; + drive_conf.compare_a = calc_speed(-forward - right); + drive_conf.compare_b = calc_speed(forward - right); + drive_pwm.set_config(&drive_conf); + + // underglow + set_underglow(&mut light_pwm_rg, &mut light_pwm_b, command.underglow); + + motor_update.await; } } @@ -158,6 +151,7 @@ async fn main(spawner: Spawner) { #[embassy_executor::task] async fn decoder(mut rx: BufferedUartRx<'static, UART1>) { info!("Reading..."); + let sender = COMMANDS.sender(); loop { let mut len = [0;4]; @@ -165,30 +159,36 @@ async fn decoder(mut rx: BufferedUartRx<'static, UART1>) { let len = u32::from_be_bytes(len) as usize; let mut buf = [0;1024]; - rx.read_exact(&mut buf[..len]).await.unwrap(); + if let Err(e) = rx.read_exact(&mut buf[..len]).await { + trace!("read fail {e:?}"); + }; - let Ok(data) = postcard::from_bytes::(&buf[..len]) else { + let Ok(data) = postcard::from_bytes::(&buf[..len]) else { error!("message decode fail"); continue; }; trace!("received {data:?}"); - COMMANDS.send(data).await; + sender.send((data, Instant::now())); } } /// Receive data from channel and send it over UART #[embassy_executor::task] -async fn telemetry(mut tx: BufferedUartTx<'static, UART1>, enabled: &'static AtomicBool, startup: &'static OnceLock<()>) { +async fn telemetry(mut tx: BufferedUartTx<'static, UART1>, enabled: &'static AtomicBool) { info!("Telemetry waiting..."); - startup.get().await; + //COMMANDS.receiver().unwrap().get().await; info!("Telemetry started..."); loop { let data = with_timeout(Duration::from_millis(20), SENSOR_DATA.receive()).await.ok(); + if !enabled.load(Ordering::Release) { + Timer::after_millis(10).await; + continue; + } + let packet = Response { - enabled: enabled.load(Ordering::Acquire), sensor_data: data, uptime_micros: Instant::now().as_micros(), }; @@ -201,7 +201,9 @@ async fn telemetry(mut tx: BufferedUartTx<'static, UART1>, enabled: &'static Ato if let Err(e) = tx.write_all(&serialized).await { error!("transport error {e:?}"); } - tx.flush().await.unwrap(); + if let Err(e) = tx.flush().await { + error!("flush err {e:?}"); + } } } @@ -231,8 +233,9 @@ async fn bus_voltage_monitor(adc: ADC, bus: PIN_28, temp: ADC_TEMP_SENSOR) { loop { let level = adc.read(&mut bus_voltage).await.unwrap(); + BUS_VOLTAGE.store(level, Ordering::Release); // empirically calculated against $20 microcenter voltmeter (10k & 33k divider circuit) - SENSOR_DATA.send(SensorData::BusVoltage(level as f32 / 251.6763848397)).await; + SENSOR_DATA.send(SensorData::BusVoltage(level as f32 / BUS_ADC_TO_VOLTS)).await; let temp = adc.read(&mut ts).await.unwrap(); SENSOR_DATA.send(SensorData::AmbientTemperature(convert_to_celsius(temp))).await; Timer::after_millis(3).await;