ank/cabinet
1
Fork 0
Code Activity

switched to a state instead of change architecture

Browse source 
southbridge currently hangs, embassy does not make
such things easy to diagnose
This commit is contained in:
Andy Killorin 2025-02-14 17:24:43 -05:00
parent b131ad8957
commit cc18f76e55
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
5 changed files with 132 additions and 152 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

29
common/src/lib.rs
View file

@ -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<u8>,
}
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<SensorData>,
pub uptime_micros: u64,
}

28
interface/src/main.rs
View file

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

6
northbridge/src/framed_codec.rs
View file

@ -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<Vec<u8>> for FramedCodec {
}
}
impl Encoder<Command> for FramedCodec {
impl Encoder<RobotState> 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:?}"))?)
}

96
northbridge/src/main.rs
View file

@ -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<ArrayQueue<Command>>, mut telem: Receiver<SensorData>) -> Result<()> {
async fn control(sender: watch::Sender<RobotState>, mut telem: Receiver<SensorData>) -> 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<ArrayQueue<Command>>, mut telem: Receiver<SensorDat
match timeout(Duration::from_millis(150), read.read_u32()).await {
Ok(it) => { break it?; },
Err(Elapsed) => {
sender.force_push(Command::Stop);
sender.send(RobotState::stopped())?;
continue;
},
};
@ -124,13 +105,13 @@ async fn control(sender: Arc<ArrayQueue<Command>>, mut telem: Receiver<SensorDat
let mut buf = vec![0; len as usize];
timeout(Duration::from_millis(100), read.read_exact(&mut buf)).await??;
let cmd: Command = postcard::from_bytes(&buf)?;
let cmd: RobotState = postcard::from_bytes(&buf)?;
sender.force_push(cmd);
sender.send(cmd)?;
}
}
async fn update_telem(state: Arc<RwLock<RobotState>>, mut telem: Receiver<SensorData>) -> Result<()> {
async fn update_telem(state: Arc<RwLock<RobotInfo>>, mut telem: Receiver<SensorData>) -> Result<()> {
loop {
let telem = telem.recv().await?;
println!("sensor {telem:?}");
@ -144,26 +125,3 @@ async fn update_telem(state: Arc<RwLock<RobotState>>, mut telem: Receiver<Sensor
}
}
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,
}
}
}

125
southbridge/src/main.rs
View file

@ -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<CriticalSectionRawMutex, Command, 5> = Channel::new();
pub static COMMANDS: Watch<CriticalSectionRawMutex, (RobotState, Instant), 5> = Watch::new();
pub static SENSOR_DATA: Channel<CriticalSectionRawMutex, SensorData, 5> = 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::<Command>(&buf[..len]) else {
let Ok(data) = postcard::from_bytes::<RobotState>(&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;