Compare commits
13 commits
6a285fe7cd
...
ad82a25329
| Author | SHA1 | Date | |
|---|---|---|---|
ad82a25329
|
|||
|
ad1f552151
|
|||
|
cc18f76e55
|
|||
|
b131ad8957
|
|||
|
0a2f22a4f1
|
|||
|
4af8cb7aa7
|
|||
|
ef93458e94
|
|||
|
1457a035ee
|
|||
|
025b05e3de
|
|||
|
bd683d9b74
|
|||
|
852b95dcc0
|
|||
|
498287aedf
|
|||
|
e4516bc804
|
12 changed files with 626 additions and 241 deletions
|
|
@ -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 const 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,
|
||||
}
|
||||
|
|
@ -37,4 +62,6 @@ pub enum SensorData {
|
|||
Magnetometer(Vector3<f32>),
|
||||
/// V
|
||||
BusVoltage(f32),
|
||||
/// C
|
||||
AmbientTemperature(f32),
|
||||
}
|
||||
|
|
|
|||
81
interface/Cargo.lock
generated
81
interface/Cargo.lock
generated
|
|
@ -74,6 +74,12 @@ version = "3.17.0"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "1628fb46dfa0b37568d12e5edd512553eccf6a22a78e8bde00bb4aed84d5bdbf"
|
||||
|
||||
[[package]]
|
||||
name = "bytemuck"
|
||||
version = "1.21.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "ef657dfab802224e671f5818e9a4935f9b1957ed18e58292690cc39e7a4092a3"
|
||||
|
||||
[[package]]
|
||||
name = "byteorder"
|
||||
version = "1.5.0"
|
||||
|
|
@ -208,7 +214,7 @@ checksum = "162ee34ebcb7c64a8abebc059ce0fee27c2262618d7b60ed8faf72fef13c3650"
|
|||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
"syn 2.0.98",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -332,6 +338,7 @@ dependencies = [
|
|||
"common",
|
||||
"futures",
|
||||
"gilrs",
|
||||
"nalgebra",
|
||||
"postcard",
|
||||
"serde",
|
||||
"tokio",
|
||||
|
|
@ -398,6 +405,16 @@ dependencies = [
|
|||
"libc",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "matrixmultiply"
|
||||
version = "0.3.9"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "9380b911e3e96d10c1f415da0876389aaf1b56759054eeb0de7df940c456ba1a"
|
||||
dependencies = [
|
||||
"autocfg",
|
||||
"rawpointer",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "memchr"
|
||||
version = "2.7.4"
|
||||
|
|
@ -431,6 +448,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
|
|||
checksum = "20bd243ab3dbb395b39ee730402d2e5405e448c75133ec49cc977762c4cba3d1"
|
||||
dependencies = [
|
||||
"approx",
|
||||
"matrixmultiply",
|
||||
"nalgebra-macros",
|
||||
"num-complex",
|
||||
"num-rational",
|
||||
"num-traits",
|
||||
|
|
@ -439,6 +458,17 @@ dependencies = [
|
|||
"typenum",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "nalgebra-macros"
|
||||
version = "0.1.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "01fcc0b8149b4632adc89ac3b7b31a12fb6099a0317a4eb2ebff574ef7de7218"
|
||||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn 1.0.109",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "nix"
|
||||
version = "0.29.0"
|
||||
|
|
@ -582,6 +612,12 @@ dependencies = [
|
|||
"proc-macro2",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "rawpointer"
|
||||
version = "0.2.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3"
|
||||
|
||||
[[package]]
|
||||
name = "redox_syscall"
|
||||
version = "0.5.8"
|
||||
|
|
@ -612,6 +648,15 @@ version = "1.0.19"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "f7c45b9784283f1b2e7fb61b42047c2fd678ef0960d4f6f1eba131594cc369d4"
|
||||
|
||||
[[package]]
|
||||
name = "safe_arch"
|
||||
version = "0.7.4"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "96b02de82ddbe1b636e6170c21be622223aea188ef2e139be0a5b219ec215323"
|
||||
dependencies = [
|
||||
"bytemuck",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "scopeguard"
|
||||
version = "1.2.0"
|
||||
|
|
@ -641,7 +686,7 @@ checksum = "5a9bf7cf98d04a2b28aead066b7496853d4779c9cc183c440dbac457641e19a0"
|
|||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
"syn 2.0.98",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -663,6 +708,7 @@ dependencies = [
|
|||
"num-complex",
|
||||
"num-traits",
|
||||
"paste",
|
||||
"wide",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -705,6 +751,17 @@ version = "1.2.0"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3"
|
||||
|
||||
[[package]]
|
||||
name = "syn"
|
||||
version = "1.0.109"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237"
|
||||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"unicode-ident",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "syn"
|
||||
version = "2.0.98"
|
||||
|
|
@ -742,7 +799,7 @@ checksum = "6e06d43f1345a3bcd39f6a56dbb7dcab2ba47e68e8ac134855e7e2bdbaf8cab8"
|
|||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
"syn 2.0.98",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -797,7 +854,7 @@ dependencies = [
|
|||
"log",
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
"syn 2.0.98",
|
||||
"wasm-bindgen-shared",
|
||||
]
|
||||
|
||||
|
|
@ -819,7 +876,7 @@ checksum = "8ae87ea40c9f689fc23f209965b6fb8a99ad69aeeb0231408be24920604395de"
|
|||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
"syn 2.0.98",
|
||||
"wasm-bindgen-backend",
|
||||
"wasm-bindgen-shared",
|
||||
]
|
||||
|
|
@ -843,6 +900,16 @@ dependencies = [
|
|||
"wasm-bindgen",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "wide"
|
||||
version = "0.7.32"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "41b5576b9a81633f3e8df296ce0063042a73507636cbe956c61133dd7034ab22"
|
||||
dependencies = [
|
||||
"bytemuck",
|
||||
"safe_arch",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "windows"
|
||||
version = "0.59.0"
|
||||
|
|
@ -874,7 +941,7 @@ checksum = "83577b051e2f49a058c308f17f273b570a6a758386fc291b5f6a934dd84e48c1"
|
|||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
"syn 2.0.98",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -885,7 +952,7 @@ checksum = "cb26fd936d991781ea39e87c3a27285081e3c0da5ca0fcbc02d368cc6f52ff01"
|
|||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
"syn 2.0.98",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
|
|||
|
|
@ -11,3 +11,4 @@ gilrs = "0.11.0"
|
|||
futures = "0.3.31"
|
||||
serde = { version = "1.0.217", features = ["alloc"] }
|
||||
postcard = { version = "1.0.0", features = ["alloc", "use-std"] }
|
||||
nalgebra = "0.31.2"
|
||||
|
|
|
|||
|
|
@ -1,9 +1,10 @@
|
|||
use std::time::Duration;
|
||||
|
||||
use anyhow::Context;
|
||||
use common::Command;
|
||||
use common::{RobotState, SensorData};
|
||||
use gilrs::{Axis, Event, Gilrs};
|
||||
use tokio::{io::{AsyncWriteExt, BufWriter}, net::TcpStream, time::sleep};
|
||||
use nalgebra::Vector3;
|
||||
use tokio::{io::{AsyncReadExt, AsyncWriteExt, BufWriter}, net::{tcp::OwnedReadHalf, TcpStream}, task::JoinHandle, time::{sleep, Instant}};
|
||||
|
||||
#[tokio::main]
|
||||
async fn main() -> anyhow::Result<()> {
|
||||
|
|
@ -13,20 +14,27 @@ 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::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 = "pelican.dyn.wpi.edu:3322";
|
||||
let mut robot = TcpStream::connect(server).await?;
|
||||
robot.set_nodelay(true)?;
|
||||
|
||||
let (telem, robot_controller) = robot.into_split();
|
||||
|
||||
let (_telem, robot_controller) = robot.split();
|
||||
tokio::spawn(print_telem(telem));
|
||||
|
||||
let mut robot_controller = BufWriter::new(robot_controller);
|
||||
|
||||
let mut active_gamepad = None;
|
||||
let mut last_event = Instant::now();
|
||||
|
||||
loop {
|
||||
while let Some(Event { id,..}) = gamepads.next_event() {
|
||||
active_gamepad = Some(id);
|
||||
last_event = Instant::now();
|
||||
}
|
||||
|
||||
let Some(gamepad) = active_gamepad else {
|
||||
|
|
@ -35,28 +43,75 @@ async fn main() -> anyhow::Result<()> {
|
|||
|
||||
let gamepad = gamepads.gamepad(gamepad);
|
||||
|
||||
|
||||
|
||||
let values = gamepad.state();
|
||||
dbg!(values);
|
||||
|
||||
let mut ly= values.axis_data(ly).map(|d| d.value()).unwrap_or(0.0);
|
||||
let mut rx= values.axis_data(rx).map(|d| d.value()).unwrap_or(0.0);
|
||||
|
||||
let boost = values.button_data(lb).map(|b| b.is_pressed()).unwrap_or(false);
|
||||
|
||||
let a = values.button_data(a).map(|b| b.is_pressed()).unwrap_or(false);
|
||||
|
||||
for axis in [&mut rx, &mut ly] {
|
||||
if axis.abs() < 0.05 {
|
||||
*axis = 0.0;
|
||||
}
|
||||
|
||||
if !boost {
|
||||
*axis *= 0.3;
|
||||
}
|
||||
}
|
||||
|
||||
let cmd = Command::Twist(ly, rx);
|
||||
if !gamepad.is_connected() {
|
||||
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 = 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?;
|
||||
|
||||
robot_controller.flush().await?;
|
||||
sleep(Duration::from_micros(3500)).await;
|
||||
if let Err(e) = robot_controller.flush().await {
|
||||
println!("flush fail: {e}, reconnecting");
|
||||
robot = TcpStream::connect(server).await?;
|
||||
robot.set_nodelay(true)?;
|
||||
|
||||
let (telem, robot) = robot.into_split();
|
||||
|
||||
tokio::spawn(print_telem(telem));
|
||||
|
||||
robot_controller = BufWriter::new(robot);
|
||||
};
|
||||
|
||||
sleep(Duration::from_millis(25)).await;
|
||||
}
|
||||
}
|
||||
|
||||
async fn print_telem(mut telem: OwnedReadHalf) -> anyhow::Result<()> {
|
||||
loop {
|
||||
let len = telem.read_u32().await? as usize;
|
||||
let mut buf = vec![0; len];
|
||||
telem.read_exact(&mut buf).await?;
|
||||
let telem: SensorData = postcard::from_bytes(&buf)?;
|
||||
|
||||
match telem {
|
||||
SensorData::BusVoltage(voltage) => {
|
||||
println!("voltage: {voltage}");
|
||||
}
|
||||
SensorData::AmbientTemperature(temp) => {
|
||||
println!("pico temp {}", temp * 9./5. + 32.);
|
||||
}
|
||||
_ => {}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
57
northbridge/Cargo.lock
generated
57
northbridge/Cargo.lock
generated
|
|
@ -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",
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -1,3 +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:
|
||||
scp target/armv7-unknown-linux-gnueabihf/release/northbridge cruise@{{server}}:
|
||||
ssh cruise@{{server}} systemctl --user restart nightstand
|
||||
|
|
|
|||
|
|
@ -1,9 +1,9 @@
|
|||
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}};
|
||||
use tokio_util::{bytes::{Buf, BytesMut}, codec::{Decoder, Encoder}};
|
||||
|
||||
pub struct FramedCodec {
|
||||
codec: framed::bytes::Codec,
|
||||
|
|
@ -21,20 +21,40 @@ impl Decoder for FramedCodec {
|
|||
type Error = anyhow::Error;
|
||||
|
||||
fn decode(&mut self, src: &mut BytesMut) -> Result<Option<Self::Item>, Self::Error> {
|
||||
println!("len: {}", src.len());
|
||||
if !src.contains(&0) {
|
||||
return Ok(None);
|
||||
}
|
||||
|
||||
let terminator_idx = src.iter().position(|b| *b == 0).unwrap();
|
||||
|
||||
let (message, remainder) = match postcard::take_from_bytes_cobs(src) {
|
||||
result::Result::Ok(v) => v,
|
||||
result::Result::Err(e) => {
|
||||
println!("telem err: {e}");
|
||||
// skip to the next terminator and try one more time
|
||||
src.advance(terminator_idx+1);
|
||||
if !src.contains(&0) {
|
||||
println!("incomplete frame");
|
||||
return Ok(None);
|
||||
}
|
||||
|
||||
match postcard::take_from_bytes_cobs(src) {
|
||||
result::Result::Ok(v) => v,
|
||||
result::Result::Err(e) => {
|
||||
src.clear();
|
||||
Err(e)?
|
||||
println!("telem err: {e}, cleared");
|
||||
return Ok(None);
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
let remainder = remainder.to_vec();
|
||||
|
||||
src.clear();
|
||||
if remainder.len() < 200 { // magic number
|
||||
src.extend_from_slice(&remainder);
|
||||
}
|
||||
|
||||
Ok(Some(message))
|
||||
}
|
||||
|
|
@ -51,10 +71,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:?}"))?)
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,92 +1,123 @@
|
|||
#![feature(async_closure)]
|
||||
|
||||
use std::time::Duration;
|
||||
use std::{sync::Arc, time::Duration};
|
||||
|
||||
use anyhow::{Context, Result};
|
||||
use common::{Command, Response, BAUDRATE};
|
||||
use common::{Response, RobotState, SensorData, BAUDRATE};
|
||||
use crossbeam::queue::ArrayQueue;
|
||||
use framed_codec::FramedCodec;
|
||||
use futures::{SinkExt, StreamExt};
|
||||
use tokio::{io::AsyncReadExt, net::{TcpListener, TcpSocket}, sync::{broadcast, watch::{self, Sender}}, task::JoinHandle, time::timeout};
|
||||
use futures::{FutureExt, SinkExt, StreamExt, TryStreamExt};
|
||||
use nalgebra::{SimdPartialOrd, SimdValue, Vector3};
|
||||
use tokio::{io::{AsyncReadExt, AsyncWriteExt}, net::{TcpListener, TcpSocket}, sync::{broadcast::{self, error::RecvError, Receiver, Sender}, watch::{self, channel }, RwLock}, task::JoinHandle, time::{error::Elapsed, sleep, timeout}};
|
||||
use tokio_serial::SerialPortBuilderExt;
|
||||
use tokio_util::codec::Framed;
|
||||
|
||||
mod framed_codec;
|
||||
|
||||
#[derive(Default)]
|
||||
struct RobotInfo {
|
||||
bus_voltage: f32,
|
||||
}
|
||||
|
||||
#[tokio::main]
|
||||
async fn main() -> Result<()> {
|
||||
let mut serial = tokio_serial::new("/dev/ttyAMA0", BAUDRATE).open_native_async()?;
|
||||
|
||||
let (sensor_sender, mut sensor_data) = broadcast::channel(5);
|
||||
let (sensor_sender, sensor_data) = broadcast::channel(5);
|
||||
|
||||
serial.set_exclusive(false)?;
|
||||
|
||||
let (mut write, mut read) = Framed::new(serial, FramedCodec::new()).split();
|
||||
|
||||
let (send, commands) = watch::channel(Command::Stop);
|
||||
let (sender, mut command_receiver) = channel(RobotState::stopped());
|
||||
|
||||
let _: JoinHandle<Result<()>> = tokio::spawn(async move {
|
||||
loop {
|
||||
println!("sensor {:?}", sensor_data.recv().await?);
|
||||
}
|
||||
});
|
||||
let state = Arc::new(RwLock::new(RobotInfo {
|
||||
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 {
|
||||
loop {
|
||||
let _ = send.send(Command::Stop);
|
||||
if let Err(e) = control(send.clone()).await {
|
||||
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?;
|
||||
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?;
|
||||
write.flush().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:?}");
|
||||
|
||||
write.send(Command::Enable).await?;
|
||||
write.flush().await?;
|
||||
|
||||
//dbg!(&data);
|
||||
|
||||
if let Some(data) = data.sensor_data {
|
||||
let _ = sensor_sender.send(data);
|
||||
}
|
||||
|
||||
write.send(dbg!(commands.borrow().clone())).await?;
|
||||
write.flush().await?;
|
||||
write.send(command.clone()).await?;
|
||||
}
|
||||
}
|
||||
|
||||
async fn control(sender: Sender<Command>) -> Result<()> {
|
||||
async fn control(sender: watch::Sender<RobotState>, mut telem: Receiver<SensorData>) -> Result<()> {
|
||||
let listener = TcpListener::bind("0.0.0.0:3322").await?;
|
||||
|
||||
let (mut stream, addr) = listener.accept().await?;
|
||||
let (stream, addr) = listener.accept().await?;
|
||||
|
||||
println!("connected to {addr:?}");
|
||||
|
||||
let (mut read, mut write) = stream.into_split();
|
||||
|
||||
let _: JoinHandle<Result<()>> = tokio::spawn(async move {
|
||||
loop {
|
||||
let data = match telem.recv().await {
|
||||
Ok(it) => it,
|
||||
Err(RecvError::Lagged(_)) => {continue;},
|
||||
Err(e) => {Err(e)?},
|
||||
};
|
||||
let data = postcard::to_stdvec(&data)?;
|
||||
write.write_u32(data.len() as u32).await?;
|
||||
write.write_all(&data).await?;
|
||||
}
|
||||
});
|
||||
|
||||
let len = timeout(Duration::from_millis(30), stream.read_u32()).await??;
|
||||
loop {
|
||||
let len = loop {
|
||||
match timeout(Duration::from_millis(150), read.read_u32()).await {
|
||||
Ok(it) => { break it?; },
|
||||
Err(Elapsed) => {
|
||||
sender.send(RobotState::stopped())?;
|
||||
continue;
|
||||
},
|
||||
};
|
||||
};
|
||||
let mut buf = vec![0; len as usize];
|
||||
timeout(Duration::from_millis(30), stream.read_exact(&mut buf)).await??;
|
||||
timeout(Duration::from_millis(100), read.read_exact(&mut buf)).await??;
|
||||
|
||||
let cmd: Command = postcard::from_bytes(&buf)?;
|
||||
|
||||
println!("recv {cmd:?}");
|
||||
let cmd: RobotState = postcard::from_bytes(&buf)?;
|
||||
|
||||
sender.send(cmd)?;
|
||||
}
|
||||
}
|
||||
|
||||
async fn update_telem(state: Arc<RwLock<RobotInfo>>, 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;
|
||||
}
|
||||
_ => {}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
267
southbridge/Cargo.lock
generated
267
southbridge/Cargo.lock
generated
|
|
@ -79,18 +79,18 @@ dependencies = [
|
|||
|
||||
[[package]]
|
||||
name = "bit-set"
|
||||
version = "0.5.3"
|
||||
version = "0.6.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "0700ddab506f33b20a03b13996eccd309a48e5ff77d0d95926aa0210fb4e95f1"
|
||||
checksum = "f0481a0e032742109b1133a095184ee93d88f3dc9e0d28a5d033dc77a073f44f"
|
||||
dependencies = [
|
||||
"bit-vec",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "bit-vec"
|
||||
version = "0.6.3"
|
||||
version = "0.7.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "349f9b6a179ed607305526ca489b34ad0a41aed5f7980fa90eb03160b69598fb"
|
||||
checksum = "d2c54ff287cfc0a34f38a6b832ea1bd8e448a330b3e40a50859e6488bee07f22"
|
||||
|
||||
[[package]]
|
||||
name = "bitfield"
|
||||
|
|
@ -110,6 +110,15 @@ version = "2.8.0"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "8f68f53c83ab957f72c32642f3868eec03eb974d1fb82e453128456482613d36"
|
||||
|
||||
[[package]]
|
||||
name = "block-buffer"
|
||||
version = "0.10.4"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "3078c7629b62d3f0439517fa394996acacc5cbc91c5a20d8c658e77abd503a71"
|
||||
dependencies = [
|
||||
"generic-array",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "bytemuck"
|
||||
version = "1.21.0"
|
||||
|
|
@ -184,6 +193,15 @@ dependencies = [
|
|||
"syn 2.0.96",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "cpufeatures"
|
||||
version = "0.2.17"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "59ed5838eebb26a2bb2e58f6d5b5316989ae9d08bab10e0e6d103e656d1b0280"
|
||||
dependencies = [
|
||||
"libc",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "crc-any"
|
||||
version = "2.5.0"
|
||||
|
|
@ -211,6 +229,16 @@ version = "0.2.3"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "43da5946c66ffcc7745f48db692ffbb10a83bfe0afd96235c5c2a4fb23994929"
|
||||
|
||||
[[package]]
|
||||
name = "crypto-common"
|
||||
version = "0.1.6"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "1bfb12502f3fc46cca1bb51ac28df9d618d813cdc3d2f25b9fe775a34af26bb3"
|
||||
dependencies = [
|
||||
"generic-array",
|
||||
"typenum",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "darling"
|
||||
version = "0.20.10"
|
||||
|
|
@ -246,6 +274,15 @@ dependencies = [
|
|||
"syn 2.0.96",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "dcmimu"
|
||||
version = "0.2.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "692b1c8923315ac22103c1890512597baf543b95f2eebd66f572ffb1ff8f0170"
|
||||
dependencies = [
|
||||
"libm 0.1.4",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "debug-helper"
|
||||
version = "0.3.13"
|
||||
|
|
@ -253,10 +290,14 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
|
|||
checksum = "f578e8e2c440e7297e008bb5486a3a8a194775224bbc23729b0dbdfaeebf162e"
|
||||
|
||||
[[package]]
|
||||
name = "diff"
|
||||
version = "0.1.13"
|
||||
name = "digest"
|
||||
version = "0.10.7"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "56254986775e3233ffa9c4d7d3faaf6d36a2c09d30b20687e9f88bc8bafc16c8"
|
||||
checksum = "9ed9a281f7bc9b7576e61468ba615a66a5c8cfdff42420a70aa82701a3b1e292"
|
||||
dependencies = [
|
||||
"block-buffer",
|
||||
"crypto-common",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "dirs-next"
|
||||
|
|
@ -297,10 +338,10 @@ checksum = "60b1af1c220855b6ceac025d3f6ecdd2b7c4894bfe9cd9bda4fbb4bc7c0d4cf0"
|
|||
[[package]]
|
||||
name = "embassy-embedded-hal"
|
||||
version = "0.3.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "41fea5ef5bed4d3468dfd44f5c9fa4cda8f54c86d4fb4ae683eacf9d39e2ea12"
|
||||
source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
|
||||
dependencies = [
|
||||
"embassy-futures",
|
||||
"embassy-hal-internal",
|
||||
"embassy-sync",
|
||||
"embassy-time",
|
||||
"embedded-hal 0.2.7",
|
||||
|
|
@ -314,8 +355,7 @@ dependencies = [
|
|||
[[package]]
|
||||
name = "embassy-executor"
|
||||
version = "0.7.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "90327bcc66333a507f89ecc4e2d911b265c45f5c9bc241f98eee076752d35ac6"
|
||||
source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
|
||||
dependencies = [
|
||||
"cortex-m",
|
||||
"critical-section",
|
||||
|
|
@ -326,8 +366,7 @@ dependencies = [
|
|||
[[package]]
|
||||
name = "embassy-executor-macros"
|
||||
version = "0.6.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "3577b1e9446f61381179a330fc5324b01d511624c55f25e3c66c9e3c626dbecf"
|
||||
source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
|
||||
dependencies = [
|
||||
"darling",
|
||||
"proc-macro2",
|
||||
|
|
@ -338,14 +377,12 @@ dependencies = [
|
|||
[[package]]
|
||||
name = "embassy-futures"
|
||||
version = "0.1.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "1f878075b9794c1e4ac788c95b728f26aa6366d32eeb10c7051389f898f7d067"
|
||||
source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
|
||||
|
||||
[[package]]
|
||||
name = "embassy-hal-internal"
|
||||
version = "0.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "0ef3bac31ec146321248a169e9c7b5799f1e0b3829c7a9b324cb4600a7438f59"
|
||||
source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
|
||||
dependencies = [
|
||||
"cortex-m",
|
||||
"critical-section",
|
||||
|
|
@ -355,14 +392,12 @@ dependencies = [
|
|||
[[package]]
|
||||
name = "embassy-net-driver"
|
||||
version = "0.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "524eb3c489760508f71360112bca70f6e53173e6fe48fc5f0efd0f5ab217751d"
|
||||
source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
|
||||
|
||||
[[package]]
|
||||
name = "embassy-net-driver-channel"
|
||||
version = "0.3.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "4818c32afec43e3cae234f324bad9a976c9aa7501022d26ff60a4017a1a006b7"
|
||||
source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
|
||||
dependencies = [
|
||||
"embassy-futures",
|
||||
"embassy-net-driver",
|
||||
|
|
@ -372,8 +407,7 @@ dependencies = [
|
|||
[[package]]
|
||||
name = "embassy-rp"
|
||||
version = "0.3.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "8cfe90d82ebbfed9de96d9edc6d63b6619c0977b570a3ab1724f76023e72773b"
|
||||
source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
|
||||
dependencies = [
|
||||
"atomic-polyfill",
|
||||
"cfg-if",
|
||||
|
|
@ -412,8 +446,7 @@ dependencies = [
|
|||
[[package]]
|
||||
name = "embassy-sync"
|
||||
version = "0.6.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "8d2c8cdff05a7a51ba0087489ea44b0b1d97a296ca6b1d6d1a33ea7423d34049"
|
||||
source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
|
||||
dependencies = [
|
||||
"cfg-if",
|
||||
"critical-section",
|
||||
|
|
@ -426,8 +459,7 @@ dependencies = [
|
|||
[[package]]
|
||||
name = "embassy-time"
|
||||
version = "0.4.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "f820157f198ada183ad62e0a66f554c610cdcd1a9f27d4b316358103ced7a1f8"
|
||||
source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
|
||||
dependencies = [
|
||||
"cfg-if",
|
||||
"critical-section",
|
||||
|
|
@ -442,8 +474,7 @@ dependencies = [
|
|||
[[package]]
|
||||
name = "embassy-time-driver"
|
||||
version = "0.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "8d45f5d833b6d98bd2aab0c2de70b18bfaa10faf661a1578fd8e5dfb15eb7eba"
|
||||
source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
|
||||
dependencies = [
|
||||
"document-features",
|
||||
]
|
||||
|
|
@ -451,8 +482,7 @@ dependencies = [
|
|||
[[package]]
|
||||
name = "embassy-time-queue-utils"
|
||||
version = "0.1.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "dc55c748d16908a65b166d09ce976575fb8852cf60ccd06174092b41064d8f83"
|
||||
source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
|
||||
dependencies = [
|
||||
"embassy-executor",
|
||||
"heapless 0.8.0",
|
||||
|
|
@ -461,8 +491,7 @@ dependencies = [
|
|||
[[package]]
|
||||
name = "embassy-usb"
|
||||
version = "0.4.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "6e651b9b7b47b514e6e6d1940a6e2e300891a2c33641917130643602a0cb6386"
|
||||
source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
|
||||
dependencies = [
|
||||
"embassy-futures",
|
||||
"embassy-net-driver-channel",
|
||||
|
|
@ -476,14 +505,12 @@ dependencies = [
|
|||
[[package]]
|
||||
name = "embassy-usb-driver"
|
||||
version = "0.1.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "4fc247028eae04174b6635104a35b1ed336aabef4654f5e87a8f32327d231970"
|
||||
source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
|
||||
|
||||
[[package]]
|
||||
name = "embassy-usb-logger"
|
||||
version = "0.4.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "ad40e3ec749d83f4b7ab0c0599c9bef0c51a73ce1c1087d8599ff2448d00c2aa"
|
||||
source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
|
||||
dependencies = [
|
||||
"embassy-futures",
|
||||
"embassy-sync",
|
||||
|
|
@ -644,6 +671,16 @@ dependencies = [
|
|||
"pin-utils",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "generic-array"
|
||||
version = "0.14.7"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "85649ca51fd72272d7821adaf274ad91c288277713d9c18820d8499a7ff69e9a"
|
||||
dependencies = [
|
||||
"typenum",
|
||||
"version_check",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "getrandom"
|
||||
version = "0.2.15"
|
||||
|
|
@ -722,12 +759,6 @@ dependencies = [
|
|||
"stable_deref_trait",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "hermit-abi"
|
||||
version = "0.4.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "fbf6a919d6cf397374f7dfeeea91d974c7c0a7221d0d0f4f20d859d329e53fcc"
|
||||
|
||||
[[package]]
|
||||
name = "ident_case"
|
||||
version = "1.0.1"
|
||||
|
|
@ -744,55 +775,53 @@ dependencies = [
|
|||
"hashbrown 0.15.2",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "is-terminal"
|
||||
version = "0.4.15"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "e19b23d53f35ce9f56aebc7d1bb4e6ac1e9c0db7ac85c8d1760c04379edced37"
|
||||
dependencies = [
|
||||
"hermit-abi",
|
||||
"libc",
|
||||
"windows-sys",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "itertools"
|
||||
version = "0.10.5"
|
||||
version = "0.13.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "b0fd2260e829bddf4cb6ea802289de2f86d6a7a690192fbe91b3f46e0f2c8473"
|
||||
checksum = "413ee7dfc52ee1a4949ceeb7dbc8a33f2d6c088194d9f922fb8318faf1f01186"
|
||||
dependencies = [
|
||||
"either",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "lalrpop"
|
||||
version = "0.19.12"
|
||||
name = "keccak"
|
||||
version = "0.1.5"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "0a1cbf952127589f2851ab2046af368fd20645491bb4b376f04b7f94d7a9837b"
|
||||
checksum = "ecc2af9a1119c51f12a14607e783cb977bde58bc069ff0c3da1095e635d70654"
|
||||
dependencies = [
|
||||
"cpufeatures",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "lalrpop"
|
||||
version = "0.21.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "6e56f323e2d610628d1f5bdd39168a774674ac7989ed67011963bb3f71edd797"
|
||||
dependencies = [
|
||||
"ascii-canvas",
|
||||
"bit-set",
|
||||
"diff",
|
||||
"ena",
|
||||
"is-terminal",
|
||||
"itertools",
|
||||
"lalrpop-util",
|
||||
"petgraph",
|
||||
"pico-args",
|
||||
"regex",
|
||||
"regex-syntax 0.6.29",
|
||||
"regex-syntax",
|
||||
"sha3",
|
||||
"string_cache",
|
||||
"term",
|
||||
"tiny-keccak",
|
||||
"unicode-xid",
|
||||
"walkdir",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "lalrpop-util"
|
||||
version = "0.19.12"
|
||||
version = "0.21.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "d3c48237b9604c5a4702de6b824e02006c3214327564636aef27c1028a8fa0ed"
|
||||
checksum = "108dc8f5dabad92c65a03523055577d847f5dcc00f3e7d3a68bc4d48e01d8fe1"
|
||||
dependencies = [
|
||||
"regex",
|
||||
"regex-automata",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -801,6 +830,12 @@ version = "0.2.169"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "b5aba8db14291edd000dfcc4d620c7ebfb122c613afb886ca8803fa4e128a20a"
|
||||
|
||||
[[package]]
|
||||
name = "libm"
|
||||
version = "0.1.4"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "7fc7aa29613bd6a620df431842069224d8bc9011086b1db4c0e0cd47fa03ec9a"
|
||||
|
||||
[[package]]
|
||||
name = "libm"
|
||||
version = "0.2.11"
|
||||
|
|
@ -851,7 +886,7 @@ version = "0.1.6"
|
|||
source = "git+https://git.ank.dev/ank/mpu6050#32604cc16c6a604fd0a4a35bf66bd9df800da18e"
|
||||
dependencies = [
|
||||
"embedded-hal 1.0.0",
|
||||
"libm",
|
||||
"libm 0.2.11",
|
||||
"nalgebra",
|
||||
]
|
||||
|
||||
|
|
@ -931,22 +966,22 @@ dependencies = [
|
|||
|
||||
[[package]]
|
||||
name = "num_enum"
|
||||
version = "0.5.11"
|
||||
version = "0.7.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "1f646caf906c20226733ed5b1374287eb97e3c2a5c227ce668c1f2ce20ae57c9"
|
||||
checksum = "4e613fc340b2220f734a8595782c551f1250e969d87d3be1ae0579e8d4065179"
|
||||
dependencies = [
|
||||
"num_enum_derive",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num_enum_derive"
|
||||
version = "0.5.11"
|
||||
version = "0.7.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "dcbff9bc912032c62bf65ef1d5aea88983b420f4f839db1e9b0c281a25c9c799"
|
||||
checksum = "af1844ef2428cc3e1cb900be36181049ef3d3193c63e43026cfe202983b27a56"
|
||||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn 1.0.109",
|
||||
"syn 2.0.96",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -1003,6 +1038,12 @@ dependencies = [
|
|||
"siphasher",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "pico-args"
|
||||
version = "0.5.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "5be167a7af36ee22fe3115051bc51f6e6c7054c9348e28deb4f49bd6f705a315"
|
||||
|
||||
[[package]]
|
||||
name = "pin-project-lite"
|
||||
version = "0.2.16"
|
||||
|
|
@ -1018,8 +1059,7 @@ checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"
|
|||
[[package]]
|
||||
name = "pio"
|
||||
version = "0.2.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "76e09694b50f89f302ed531c1f2a7569f0be5867aee4ab4f8f729bbeec0078e3"
|
||||
source = "git+https://github.com/rp-rs/pio-rs?rev=fa586448b0b223217eec8c92c19fe6823dd04cc4#fa586448b0b223217eec8c92c19fe6823dd04cc4"
|
||||
dependencies = [
|
||||
"arrayvec",
|
||||
"num_enum",
|
||||
|
|
@ -1029,30 +1069,26 @@ dependencies = [
|
|||
[[package]]
|
||||
name = "pio-parser"
|
||||
version = "0.2.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "77532c2b8279aef98dfc7207ef15298a5a3d6b6cc76ccc8b65913d69f3a8dd6b"
|
||||
source = "git+https://github.com/rp-rs/pio-rs?rev=fa586448b0b223217eec8c92c19fe6823dd04cc4#fa586448b0b223217eec8c92c19fe6823dd04cc4"
|
||||
dependencies = [
|
||||
"lalrpop",
|
||||
"lalrpop-util",
|
||||
"pio",
|
||||
"regex-syntax 0.6.29",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "pio-proc"
|
||||
version = "0.2.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "6b04dc870fb3a4fd8b3e4ca8c61b53bc8ac4eb78b66805d2b3c2e5c4829e0d7a"
|
||||
source = "git+https://github.com/rp-rs/pio-rs?rev=fa586448b0b223217eec8c92c19fe6823dd04cc4#fa586448b0b223217eec8c92c19fe6823dd04cc4"
|
||||
dependencies = [
|
||||
"codespan-reporting",
|
||||
"lalrpop-util",
|
||||
"pio",
|
||||
"pio-parser",
|
||||
"proc-macro-error",
|
||||
"proc-macro-error2",
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"regex-syntax 0.6.29",
|
||||
"syn 1.0.109",
|
||||
"syn 2.0.96",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -1082,27 +1118,25 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
|
|||
checksum = "925383efa346730478fb4838dbe9137d2a47675ad789c546d150a6e1dd4ab31c"
|
||||
|
||||
[[package]]
|
||||
name = "proc-macro-error"
|
||||
version = "1.0.4"
|
||||
name = "proc-macro-error-attr2"
|
||||
version = "2.0.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c"
|
||||
checksum = "96de42df36bb9bba5542fe9f1a054b8cc87e172759a1868aa05c1f3acc89dfc5"
|
||||
dependencies = [
|
||||
"proc-macro-error-attr",
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn 1.0.109",
|
||||
"version_check",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "proc-macro-error-attr"
|
||||
version = "1.0.4"
|
||||
name = "proc-macro-error2"
|
||||
version = "2.0.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869"
|
||||
checksum = "11ec05c52be0a07b08061f7dd003e7d7092e0472bc731b4af7bb1ef876109802"
|
||||
dependencies = [
|
||||
"proc-macro-error-attr2",
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"version_check",
|
||||
"syn 2.0.96",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -1164,7 +1198,7 @@ dependencies = [
|
|||
"aho-corasick",
|
||||
"memchr",
|
||||
"regex-automata",
|
||||
"regex-syntax 0.8.5",
|
||||
"regex-syntax",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -1175,15 +1209,9 @@ checksum = "809e8dc61f6de73b46c85f4c96486310fe304c434cfa43669d7b40f711150908"
|
|||
dependencies = [
|
||||
"aho-corasick",
|
||||
"memchr",
|
||||
"regex-syntax 0.8.5",
|
||||
"regex-syntax",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "regex-syntax"
|
||||
version = "0.6.29"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "f162c6dd7b008981e4d40210aca20b4bd0f9b60ca9271061b07f78537722f2e1"
|
||||
|
||||
[[package]]
|
||||
name = "regex-syntax"
|
||||
version = "0.8.5"
|
||||
|
|
@ -1248,6 +1276,15 @@ version = "1.0.19"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "f7c45b9784283f1b2e7fb61b42047c2fd678ef0960d4f6f1eba131594cc369d4"
|
||||
|
||||
[[package]]
|
||||
name = "same-file"
|
||||
version = "1.0.6"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "93fc1dc3aaa9bfed95e02e6eadabb4baf7e3078b0bd1b4d7b6b0b68378900502"
|
||||
dependencies = [
|
||||
"winapi-util",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "scopeguard"
|
||||
version = "1.2.0"
|
||||
|
|
@ -1301,6 +1338,16 @@ version = "0.1.0"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "5f179d4e11094a893b82fff208f74d448a7512f99f5a0acbd5c679b705f83ed9"
|
||||
|
||||
[[package]]
|
||||
name = "sha3"
|
||||
version = "0.10.8"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "75872d278a8f37ef87fa0ddbda7802605cb18344497949862c0d4dcb291eba60"
|
||||
dependencies = [
|
||||
"digest",
|
||||
"keccak",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "simba"
|
||||
version = "0.7.3"
|
||||
|
|
@ -1350,6 +1397,7 @@ dependencies = [
|
|||
"common",
|
||||
"cortex-m",
|
||||
"cortex-m-rt",
|
||||
"dcmimu",
|
||||
"embassy-executor",
|
||||
"embassy-rp",
|
||||
"embassy-sync",
|
||||
|
|
@ -1486,15 +1534,6 @@ dependencies = [
|
|||
"syn 2.0.96",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "tiny-keccak"
|
||||
version = "2.0.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "2c9d3793400a45f954c52e73d068316d76b6f4e36977e3fcebb13a2721e80237"
|
||||
dependencies = [
|
||||
"crunchy",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "typenum"
|
||||
version = "1.17.0"
|
||||
|
|
@ -1593,6 +1632,16 @@ dependencies = [
|
|||
"vcell",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "walkdir"
|
||||
version = "2.5.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "29790946404f91d9c5d06f9874efddea1dc06c5efe94541a7d6863108e3a5e4b"
|
||||
dependencies = [
|
||||
"same-file",
|
||||
"winapi-util",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "wasi"
|
||||
version = "0.11.0+wasi-snapshot-preview1"
|
||||
|
|
|
|||
|
|
@ -25,9 +25,18 @@ log = "0.4"
|
|||
framed = {git = "https://git.ank.dev/ank/framed-rs", default-features=false, features = ["use_nightly"]}
|
||||
common = { path = "../common" }
|
||||
mpu6050 = { git = "https://git.ank.dev/ank/mpu6050" }
|
||||
dcmimu = "0.2.2"
|
||||
nalgebra = { version = "0.31.2", default-features=false, features = ["serde-serialize-no-std"] }
|
||||
serde = { version = "1.0.203", default-features = false, features = ["derive"] }
|
||||
postcard = "1.0.0"
|
||||
static_cell = "2.1"
|
||||
portable-atomic = { version = "1.5", features = ["critical-section"] }
|
||||
heapless = "0.8"
|
||||
|
||||
[patch.crates-io]
|
||||
# https://github.com/embassy-rs/embassy/pull/3763/
|
||||
embassy-rp = { git = 'https://github.com/embassy-rs/embassy/' }
|
||||
embassy-time = { git = 'https://github.com/embassy-rs/embassy/' }
|
||||
embassy-usb-logger = { git = 'https://github.com/embassy-rs/embassy/' }
|
||||
embassy-sync = { git = 'https://github.com/embassy-rs/embassy/' }
|
||||
embassy-executor = { git = 'https://github.com/embassy-rs/embassy/' }
|
||||
|
|
|
|||
|
|
@ -1,16 +1,16 @@
|
|||
#![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::{bind_interrupts, block::ImageDef, gpio::{Level, Output}, peripherals::{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_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, mutex::Mutex, once_lock::OnceLock, pubsub::PubSubChannel, signal::Signal, 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};
|
||||
use nalgebra::clamp;
|
||||
use nalgebra::{clamp, Vector3};
|
||||
use portable_atomic::AtomicBool;
|
||||
use static_cell::{ConstStaticCell, StaticCell};
|
||||
|
||||
|
|
@ -18,18 +18,27 @@ bind_interrupts!(struct Irqs {
|
|||
I2C1_IRQ => embassy_rp::i2c::InterruptHandler<embassy_rp::peripherals::I2C1>;
|
||||
USBCTRL_IRQ => embassy_rp::usb::InterruptHandler<USB>;
|
||||
UART1_IRQ => BufferedInterruptHandler<UART1>;
|
||||
ADC_IRQ_FIFO => adc::InterruptHandler;
|
||||
});
|
||||
|
||||
pub static COMMANDS: Channel<CriticalSectionRawMutex, Command, 5> = Channel::new();
|
||||
pub static COMMANDS: Watch<CriticalSectionRawMutex, (RobotState, Instant), 3> = 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();
|
||||
|
||||
#[embassy_executor::task]
|
||||
async fn logger_task(driver: Driver<'static, USB>) {
|
||||
embassy_usb_logger::run!(1024, log::LevelFilter::Trace, driver);
|
||||
embassy_usb_logger::run!(2048, log::LevelFilter::Trace, driver);
|
||||
}
|
||||
|
||||
#[panic_handler]
|
||||
|
|
@ -48,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();
|
||||
|
|
@ -57,12 +67,33 @@ async fn main(spawner: Spawner) {
|
|||
let stopped = drive_conf.clone();
|
||||
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();
|
||||
light_conf_rg.top = 255;
|
||||
light_conf_rg.compare_b = 0;
|
||||
light_conf_rg.compare_a = 0;
|
||||
let mut light_pwm_rg = Pwm::new_output_ab(p.PWM_SLICE7, p.PIN_14, p.PIN_15, light_conf_rg);
|
||||
|
||||
let mut light_conf_b: pwm::Config = Default::default();
|
||||
light_conf_b.divider = 1.into();
|
||||
light_conf_b.top = 255;
|
||||
light_conf_b.compare_b = 0;
|
||||
let mut light_pwm_b = Pwm::new_output_b(p.PWM_SLICE6, p.PIN_13, light_conf_b);
|
||||
set_underglow(&mut light_pwm_rg, &mut light_pwm_b, disabled_orange.clone());
|
||||
|
||||
Timer::after_secs(13).await; // pi 0 serial ports act strange during the boot process
|
||||
|
||||
spawner.spawn(bus_voltage_monitor(p.ADC, p.PIN_28, p.ADC_TEMP_SENSOR)).unwrap();
|
||||
|
||||
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();
|
||||
|
|
@ -73,58 +104,41 @@ async fn main(spawner: Spawner) {
|
|||
|
||||
spawner.spawn(decoder(rx)).unwrap();
|
||||
|
||||
static STARTUP: OnceLock<()> = OnceLock::new();
|
||||
spawner.spawn(telemetry(tx)).unwrap();
|
||||
|
||||
static ENABLED: AtomicBool = AtomicBool::new(false);
|
||||
spawner.spawn(telemetry(tx, &ENABLED, &STARTUP)).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);
|
||||
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());
|
||||
commands.changed().await;
|
||||
set_underglow(&mut light_pwm_rg, &mut light_pwm_b, enabled_green.clone());
|
||||
continue;
|
||||
}
|
||||
|
||||
STARTUP.get_or_init(||());
|
||||
|
||||
match command {
|
||||
Command::Twist(forward, right) => {
|
||||
// 100% effort at 9.5v, 0% effort at 7.5v
|
||||
let brownout = ((bus_voltage()-7.5)/2.0).clamp(0., 1.);
|
||||
let command = command.clone().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);
|
||||
},
|
||||
Command::Stop => {
|
||||
drive_pwm.set_config(&stopped);
|
||||
},
|
||||
Command::Enable => {
|
||||
enabled.store(true, Ordering::Release);
|
||||
enable_watchdog = Instant::now();
|
||||
},
|
||||
Command::Disable => {
|
||||
enabled.store(false, Ordering::Release);
|
||||
drive_pwm.set_config(&stopped);
|
||||
},
|
||||
Command::FeedWatchdog => {
|
||||
enable_watchdog = Instant::now();
|
||||
}
|
||||
c => { error!("{c:?} unimplemented") }
|
||||
};
|
||||
|
||||
info!("Blink");
|
||||
// underglow
|
||||
set_underglow(&mut light_pwm_rg, &mut light_pwm_b, command.underglow);
|
||||
|
||||
motor_update.await;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -132,6 +146,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];
|
||||
|
|
@ -139,30 +154,31 @@ 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>) {
|
||||
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();
|
||||
|
||||
let packet = Response {
|
||||
enabled: enabled.load(Ordering::Acquire),
|
||||
sensor_data: data,
|
||||
uptime_micros: Instant::now().as_micros(),
|
||||
};
|
||||
|
|
@ -175,7 +191,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:?}");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -188,3 +206,50 @@ fn calc_speed(speed: f32) -> u16 {
|
|||
|
||||
(COUNTS_PER_MS * ms) as u16
|
||||
}
|
||||
|
||||
#[embassy_executor::task]
|
||||
async fn bus_voltage_monitor(adc: ADC, bus: PIN_28, temp: ADC_TEMP_SENSOR) {
|
||||
let mut adc = Adc::new(adc, Irqs, adc::Config::default());
|
||||
let mut bus_voltage = adc::Channel::new_pin(bus, Pull::None);
|
||||
let mut ts = adc::Channel::new_temp_sensor(temp);
|
||||
|
||||
// from embassy examples
|
||||
fn convert_to_celsius(raw_temp: u16) -> f32 {
|
||||
let temp = 27.0 - (raw_temp as f32 * 3.3 / 4096.0 - 0.706) / 0.001721;
|
||||
let sign = if temp < 0.0 { -1.0 } else { 1.0 };
|
||||
let rounded_temp_x10: i16 = ((temp * 10.0) + 0.5 * sign) as i16;
|
||||
(rounded_temp_x10 as f32) / 10.0
|
||||
}
|
||||
|
||||
let mut i = 0;
|
||||
|
||||
loop {
|
||||
i += 1;
|
||||
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)
|
||||
let temp = adc.read(&mut ts).await.unwrap();
|
||||
|
||||
if i % 10 == 0 { // only send telemetry every 30 milliseconds
|
||||
SENSOR_DATA.send(SensorData::BusVoltage(level as f32 / BUS_ADC_TO_VOLTS)).await;
|
||||
SENSOR_DATA.send(SensorData::AmbientTemperature(convert_to_celsius(temp))).await;
|
||||
}
|
||||
|
||||
Timer::after_millis(3).await;
|
||||
}
|
||||
}
|
||||
|
||||
fn set_underglow(rg: &mut Pwm, b: &mut Pwm, color: Vector3<u8>) {
|
||||
let mut light_conf_rg: pwm::Config = Default::default();
|
||||
light_conf_rg.divider = 1.into();
|
||||
light_conf_rg.top = 255;
|
||||
light_conf_rg.compare_b = color[1] as u16;
|
||||
light_conf_rg.compare_a = color[0] as u16;
|
||||
rg.set_config(&light_conf_rg);
|
||||
|
||||
let mut light_conf_b: pwm::Config = Default::default();
|
||||
light_conf_b.divider = 1.into();
|
||||
light_conf_b.top = 255;
|
||||
light_conf_b.compare_b = color[2] as u16;
|
||||
b.set_config(&light_conf_b);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue