ank/cabinet
1
Fork 0
Code Activity

two types of command at the same time (sputters, lags, then dies)

Browse source
This commit is contained in:
Andy Killorin 2025-02-13 15:57:34 -05:00
parent 0a2f22a4f1
commit b131ad8957
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
6 changed files with 104 additions and 27 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

30
interface/src/main.rs
View file

@ -14,10 +14,12 @@ async fn main() -> anyhow::Result<()> {
let (_, gamepad) = gamepads.gamepads().nth(0).context("no gamepads")?;
let ly = gamepad.axis_code(Axis::LeftStickY).context("no left joystick")?;
let rx = gamepad.axis_code(Axis::RightStickX).context("no right joystick")?;
let lb = gamepad.button_code(gilrs::Button::LeftTrigger2).context("no lb")?;
let lb = gamepad.button_code(gilrs::Button::LeftTrigger).context("no lb")?;
let a = gamepad.button_code(gilrs::Button::South).context("no a")?;
let mut last_a = false;
let mut robot = TcpStream::connect("pelican.dyn.wpi.edu:3322").await?;
let server = "130.215.211.79:3322";
let mut robot = TcpStream::connect(server).await?;
robot.set_nodelay(true)?;
let (telem, robot_controller) = robot.into_split();
@ -66,16 +68,27 @@ async fn main() -> anyhow::Result<()> {
}
let cmd = Command::Twist(ly, rx);
println!("twist");
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("pelican.dyn.wpi.edu:3322").await?;
robot = TcpStream::connect(server).await?;
robot.set_nodelay(true)?;
let (telem, robot) = robot.into_split();
@ -85,16 +98,7 @@ async fn main() -> anyhow::Result<()> {
robot_controller = BufWriter::new(robot);
};
let black = Vector3::repeat(0u8);
let green = Vector3::new(100u8, 0, 0);
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?;
sleep(Duration::from_micros(350)).await;
robot_controller.flush().await?;
sleep(Duration::from_micros(3500)).await;
sleep(Duration::from_millis(65)).await;
}
}

57
northbridge/Cargo.lock generated
View file

@ -146,6 +146,62 @@ version = "1.2.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b"
[[package]]
name = "crossbeam"
version = "0.8.4"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "1137cd7e7fc0fb5d3c5a8678be38ec56e819125d8d7907411fe24ccb943faca8"
dependencies = [
"crossbeam-channel",
"crossbeam-deque",
"crossbeam-epoch",
"crossbeam-queue",
"crossbeam-utils",
]
[[package]]
name = "crossbeam-channel"
version = "0.5.14"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "06ba6d68e24814cb8de6bb986db8222d3a027d15872cabc0d18817bc3c0e4471"
dependencies = [
"crossbeam-utils",
]
[[package]]
name = "crossbeam-deque"
version = "0.8.6"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "9dd111b7b7f7d55b72c0a6ae361660ee5853c9af73f70c3c2ef6858b950e2e51"
dependencies = [
"crossbeam-epoch",
"crossbeam-utils",
]
[[package]]
name = "crossbeam-epoch"
version = "0.9.18"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "5b82ac4a3c2ca9c3460964f020e1402edd5753411d7737aa39c3714ad1b5420e"
dependencies = [
"crossbeam-utils",
]
[[package]]
name = "crossbeam-queue"
version = "0.3.12"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "0f58bbc28f91df819d0aa2a2c00cd19754769c2fad90579b3592b1c9ba7a3115"
dependencies = [
"crossbeam-utils",
]
[[package]]
name = "crossbeam-utils"
version = "0.8.21"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "d0a5c400df2834b80a4c3327b3aad3a4c4cd4de0629063962b03235697506a28"
[[package]]
name = "embedded-io"
version = "0.4.0"
@ -443,6 +499,7 @@ version = "0.1.0"
dependencies = [
"anyhow",
"common",
"crossbeam",
"framed",
"futures",
"futures-core",

1
northbridge/Cargo.toml
View file

@ -15,3 +15,4 @@ nalgebra = { version = "0.31.2", default-features=false, features = ["serde-seri
tokio-util = { version = "0.7.13", features = ["codec"] }
futures-core = "0.3.31"
futures = "0.3.31"
crossbeam = "0.8.4"

6
northbridge/justfile
View file

@ -1,4 +1,6 @@
server := 'pelican.dyn.wpi.edu'
deploy:
cross build --target=armv7-unknown-linux-gnueabihf --release
scp target/armv7-unknown-linux-gnueabihf/release/northbridge cruise@pelican.dyn.wpi.edu:
ssh cruise@pelican.dyn.wpi.edu systemctl --user restart nightstand
scp target/armv7-unknown-linux-gnueabihf/release/northbridge cruise@{{server}}:
ssh cruise@{{server}} systemctl --user restart nightstand

4
northbridge/src/framed_codec.rs
View file

@ -36,7 +36,9 @@ impl Decoder for FramedCodec {
let remainder = remainder.to_vec();
src.clear();
src.extend_from_slice(&remainder);
if remainder.len() < 200 { // magic number
src.extend_from_slice(&remainder);
}
Ok(Some(message))
}

31
northbridge/src/main.rs
View file

@ -4,10 +4,11 @@ use std::{sync::Arc, time::Duration};
use anyhow::{Context, Result};
use common::{Command, Response, SensorData, BAUDRATE};
use crossbeam::queue::ArrayQueue;
use framed_codec::FramedCodec;
use futures::{SinkExt, StreamExt};
use futures::{SinkExt, StreamExt, TryStreamExt};
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::{io::{AsyncReadExt, AsyncWriteExt}, net::{TcpListener, TcpSocket}, sync::{broadcast::{self, error::RecvError, Sender, Receiver}, watch::{self }, RwLock}, task::JoinHandle, time::{error::Elapsed, timeout}};
use tokio_serial::SerialPortBuilderExt;
use tokio_util::codec::Framed;
@ -28,7 +29,7 @@ async fn main() -> Result<()> {
let (mut write, mut read) = Framed::new(serial, FramedCodec::new()).split();
let (send, commands) = watch::channel(Command::Stop);
let commands = Arc::new(ArrayQueue::new(5));
let state = Arc::new(RwLock::new(RobotState {
bus_voltage: f32::MAX,
@ -38,10 +39,11 @@ 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 _ = send.send(Command::Stop);
if let Err(e) = control(send.clone(), control_telem.resubscribe()).await {
let _ = sender.force_push(Command::Stop);
if let Err(e) = control(sender.clone(), control_telem.resubscribe()).await {
println!("controller exited: {e}");
}
}
@ -58,7 +60,6 @@ async fn main() -> Result<()> {
};
if data.enabled {
write.send(Command::FeedWatchdog).await?;
write.flush().await?;
} else {
println!("enabled southbridge");
@ -74,12 +75,21 @@ async fn main() -> Result<()> {
// 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().brownout(brownout)).await?;
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?;
}
}
async fn control(sender: Sender<Command>, mut telem: Receiver<SensorData>) -> Result<()> {
async fn control(sender: Arc<ArrayQueue<Command>>, mut telem: Receiver<SensorData>) -> Result<()> {
let listener = TcpListener::bind("0.0.0.0:3322").await?;
let (stream, addr) = listener.accept().await?;
@ -106,7 +116,8 @@ async fn control(sender: Sender<Command>, mut telem: Receiver<SensorData>) -> Re
match timeout(Duration::from_millis(150), read.read_u32()).await {
Ok(it) => { break it?; },
Err(Elapsed) => {
sender.send(Command::Stop)?;
sender.force_push(Command::Stop);
continue;
},
};
};
@ -115,7 +126,7 @@ async fn control(sender: Sender<Command>, mut telem: Receiver<SensorData>) -> Re
let cmd: Command = postcard::from_bytes(&buf)?;
sender.send(cmd)?;
sender.force_push(cmd);
}
}