Compare commits
7 commits
57522c26cc
...
b063a41783
| Author | SHA1 | Date | |
|---|---|---|---|
b063a41783
|
|||
|
99f0ed69e4
|
|||
|
02f6efaaa5
|
|||
|
b82c6dbed8
|
|||
|
31c847f39a
|
|||
|
6192627e25
|
|||
|
782fd891a1
|
6 changed files with 420 additions and 59 deletions
266
common/Cargo.lock
generated
Normal file
266
common/Cargo.lock
generated
Normal file
|
|
@ -0,0 +1,266 @@
|
|||
# This file is automatically @generated by Cargo.
|
||||
# It is not intended for manual editing.
|
||||
version = 4
|
||||
|
||||
[[package]]
|
||||
name = "approx"
|
||||
version = "0.5.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6"
|
||||
dependencies = [
|
||||
"num-traits",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "atomic-polyfill"
|
||||
version = "1.0.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "8cf2bce30dfe09ef0bfaef228b9d414faaf7e563035494d7fe092dba54b300f4"
|
||||
dependencies = [
|
||||
"critical-section",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "autocfg"
|
||||
version = "1.4.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26"
|
||||
|
||||
[[package]]
|
||||
name = "byteorder"
|
||||
version = "1.5.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b"
|
||||
|
||||
[[package]]
|
||||
name = "cobs"
|
||||
version = "0.2.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "67ba02a97a2bd10f4b59b25c7973101c79642302776489e030cd13cdab09ed15"
|
||||
|
||||
[[package]]
|
||||
name = "common"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"nalgebra",
|
||||
"postcard",
|
||||
"serde",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "critical-section"
|
||||
version = "1.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b"
|
||||
|
||||
[[package]]
|
||||
name = "hash32"
|
||||
version = "0.2.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "b0c35f58762feb77d74ebe43bdbc3210f09be9fe6742234d573bacc26ed92b67"
|
||||
dependencies = [
|
||||
"byteorder",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "heapless"
|
||||
version = "0.7.17"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "cdc6457c0eb62c71aac4bc17216026d8410337c4126773b9c5daba343f17964f"
|
||||
dependencies = [
|
||||
"atomic-polyfill",
|
||||
"hash32",
|
||||
"rustc_version",
|
||||
"serde",
|
||||
"spin",
|
||||
"stable_deref_trait",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "lock_api"
|
||||
version = "0.4.12"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "07af8b9cdd281b7915f413fa73f29ebd5d55d0d3f0155584dade1ff18cea1b17"
|
||||
dependencies = [
|
||||
"autocfg",
|
||||
"scopeguard",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "nalgebra"
|
||||
version = "0.31.4"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "20bd243ab3dbb395b39ee730402d2e5405e448c75133ec49cc977762c4cba3d1"
|
||||
dependencies = [
|
||||
"approx",
|
||||
"num-complex",
|
||||
"num-rational",
|
||||
"num-traits",
|
||||
"serde",
|
||||
"simba",
|
||||
"typenum",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num-complex"
|
||||
version = "0.4.6"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495"
|
||||
dependencies = [
|
||||
"num-traits",
|
||||
"serde",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num-integer"
|
||||
version = "0.1.46"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f"
|
||||
dependencies = [
|
||||
"num-traits",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num-rational"
|
||||
version = "0.4.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824"
|
||||
dependencies = [
|
||||
"num-integer",
|
||||
"num-traits",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num-traits"
|
||||
version = "0.2.19"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841"
|
||||
dependencies = [
|
||||
"autocfg",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "paste"
|
||||
version = "1.0.15"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a"
|
||||
|
||||
[[package]]
|
||||
name = "postcard"
|
||||
version = "1.1.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "170a2601f67cc9dba8edd8c4870b15f71a6a2dc196daec8c83f72b59dff628a8"
|
||||
dependencies = [
|
||||
"cobs",
|
||||
"heapless",
|
||||
"serde",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "proc-macro2"
|
||||
version = "1.0.93"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "60946a68e5f9d28b0dc1c21bb8a97ee7d018a8b322fa57838ba31cc878e22d99"
|
||||
dependencies = [
|
||||
"unicode-ident",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "quote"
|
||||
version = "1.0.38"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "0e4dccaaaf89514f546c693ddc140f729f958c247918a13380cccc6078391acc"
|
||||
dependencies = [
|
||||
"proc-macro2",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "rustc_version"
|
||||
version = "0.4.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "cfcb3a22ef46e85b45de6ee7e79d063319ebb6594faafcf1c225ea92ab6e9b92"
|
||||
dependencies = [
|
||||
"semver",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "scopeguard"
|
||||
version = "1.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49"
|
||||
|
||||
[[package]]
|
||||
name = "semver"
|
||||
version = "1.0.24"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "3cb6eb87a131f756572d7fb904f6e7b68633f09cca868c5df1c4b8d1a694bbba"
|
||||
|
||||
[[package]]
|
||||
name = "serde"
|
||||
version = "1.0.217"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "02fc4265df13d6fa1d00ecff087228cc0a2b5f3c0e87e258d8b94a156e984c70"
|
||||
dependencies = [
|
||||
"serde_derive",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "serde_derive"
|
||||
version = "1.0.217"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "5a9bf7cf98d04a2b28aead066b7496853d4779c9cc183c440dbac457641e19a0"
|
||||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "simba"
|
||||
version = "0.7.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "2f3fd720c48c53cace224ae62bef1bbff363a70c68c4802a78b5cc6159618176"
|
||||
dependencies = [
|
||||
"approx",
|
||||
"num-complex",
|
||||
"num-traits",
|
||||
"paste",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "spin"
|
||||
version = "0.9.8"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "6980e8d7511241f8acf4aebddbb1ff938df5eebe98691418c4468d0b72a96a67"
|
||||
dependencies = [
|
||||
"lock_api",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "stable_deref_trait"
|
||||
version = "1.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3"
|
||||
|
||||
[[package]]
|
||||
name = "syn"
|
||||
version = "2.0.96"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "d5d0adab1ae378d7f53bdebc67a39f1f151407ef230f0ce2883572f5d8985c80"
|
||||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"unicode-ident",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "typenum"
|
||||
version = "1.17.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "42ff0bf0c66b8238c6f3b578df37d0b7848e55df8577b3f74f92a69acceeb825"
|
||||
|
||||
[[package]]
|
||||
name = "unicode-ident"
|
||||
version = "1.0.14"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "adb9e6ca4f869e1180728b7950e35922a7fc6397f7b641499e8f3ef06e50dc83"
|
||||
9
common/Cargo.toml
Normal file
9
common/Cargo.toml
Normal file
|
|
@ -0,0 +1,9 @@
|
|||
[package]
|
||||
name = "common"
|
||||
version = "0.1.0"
|
||||
edition = "2021"
|
||||
|
||||
[dependencies]
|
||||
serde = { version = "1.0.*", default-features = false }
|
||||
postcard = "1.0.0"
|
||||
nalgebra = { version = "0.31.2", default-features=false, features = ["serde-serialize-no-std"]}
|
||||
54
common/src/lib.rs
Normal file
54
common/src/lib.rs
Normal file
|
|
@ -0,0 +1,54 @@
|
|||
#![no_std]
|
||||
|
||||
use nalgebra::Vector3;
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
/// -1..1
|
||||
pub type Speed = f32;
|
||||
|
||||
#[derive(Serialize, Deserialize, Debug)]
|
||||
pub enum ControlPacket {
|
||||
/// Forward, Clockwise
|
||||
Twist(Speed, Speed),
|
||||
/// Launch the flipper once
|
||||
Fire,
|
||||
/// Run the flipper motor manually
|
||||
FireOverride(Speed),
|
||||
/// Enable/Disable flipper staging
|
||||
Arm(bool),
|
||||
/// Enable/Disable auto light
|
||||
SetAuto(bool),
|
||||
RunAuto,
|
||||
/// Stop all motors
|
||||
Stop,
|
||||
/// Permanently stop all motors
|
||||
EStop,
|
||||
}
|
||||
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct SensorData {
|
||||
/// left distance, mm
|
||||
pub tof_l: Option<u16>,
|
||||
/// right distance, mm
|
||||
pub tof_r: Option<u16>,
|
||||
/// left side distance, mm
|
||||
pub tof_s: Option<u16>,
|
||||
/// acceleration, rad/s
|
||||
pub gyro: Option<Vector3<f32>>,
|
||||
/// acceleration, g
|
||||
pub accel: Option<Vector3<f32>>,
|
||||
}
|
||||
|
||||
#[derive(Serialize, Deserialize, Debug)]
|
||||
pub struct TelemetryPacket {
|
||||
pub sensors: SensorData,
|
||||
pub cam_state: CamState,
|
||||
}
|
||||
|
||||
#[derive(Serialize, Deserialize, Debug)]
|
||||
pub enum CamState {
|
||||
Firing,
|
||||
Charged,
|
||||
Charging,
|
||||
Idle,
|
||||
}
|
||||
|
|
@ -88,7 +88,8 @@ async fn main(spawner: Spawner) {
|
|||
let driver = Driver::new(p.USB, Irqs);
|
||||
spawner.spawn(logger_task(driver)).unwrap();
|
||||
|
||||
let limit_switch = Input::new(p.PIN_22, Pull::Up);
|
||||
let limit_switch = Input::new(p.PIN_16, Pull::Up);
|
||||
let mut auto_status = Output::new(p.PIN_2, Level::Low);
|
||||
|
||||
let mut d: pwm::Config = Default::default();
|
||||
d.divider = 40.into();
|
||||
|
|
@ -98,9 +99,9 @@ async fn main(spawner: Spawner) {
|
|||
let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE1, p.PIN_18, p.PIN_19, d.clone());
|
||||
|
||||
let mut f: pwm::Config = Default::default();
|
||||
f.divider = 40.into();
|
||||
f.top = 62500; // 20ms
|
||||
f.compare_b = 4687; // 1.5ms
|
||||
f.divider = 1.into(); // no reason to slow down, this is going right into a mosfet
|
||||
f.top = u16::MAX;
|
||||
f.compare_b = 0;
|
||||
let flip_pwm = Pwm::new_output_b(p.PWM_SLICE0, p.PIN_17, f.clone());
|
||||
|
||||
let sda = p.PIN_20;
|
||||
|
|
@ -112,12 +113,14 @@ async fn main(spawner: Spawner) {
|
|||
let scl = p.PIN_27;
|
||||
let config = embassy_rp::i2c::Config::default();
|
||||
let bus1 = embassy_rp::i2c::I2c::new_async(p.I2C1, scl, sda, Irqs, config);
|
||||
let tof2enable = Output::new(p.PIN_16, Level::Low);
|
||||
let tof_r_enable = Output::new(p.PIN_28, Level::Low);
|
||||
let tof_l_enable = Output::new(p.PIN_22, Level::Low);
|
||||
|
||||
let hardware = SensorHardware {
|
||||
bus_tof: unsafe {transmute(bus)},
|
||||
bus_gyro: unsafe {transmute(bus1)},
|
||||
tof2enable,
|
||||
bus_tof: unsafe {transmute(bus1)},
|
||||
bus_gyro: unsafe {transmute(bus)},
|
||||
tof_l_enable,
|
||||
tof_r_enable,
|
||||
};
|
||||
|
||||
spawn_core1(
|
||||
|
|
@ -164,7 +167,7 @@ async fn main(spawner: Spawner) {
|
|||
|
||||
// Use a link-local address for communication without DHCP server
|
||||
let config = Config::ipv4_static(embassy_net::StaticConfigV4 {
|
||||
address: embassy_net::Ipv4Cidr::new(embassy_net::Ipv4Address::new(169, 254, 1, 1), 16),
|
||||
address: embassy_net::Ipv4Cidr::new(embassy_net::Ipv4Address::new(192, 168, 1, 2), 16),
|
||||
dns_servers: heapless::Vec::new(),
|
||||
gateway: None,
|
||||
});
|
||||
|
|
@ -247,8 +250,8 @@ async fn main(spawner: Spawner) {
|
|||
info!("left to {}", clamp(forward+right, -1., 1.));
|
||||
info!("right to {}", clamp(forward-right, -1., 1.));
|
||||
|
||||
c.compare_a = calc_speed(forward - right);
|
||||
c.compare_b = calc_speed(forward + right);
|
||||
c.compare_a = calc_speed(-forward - right);
|
||||
c.compare_b = calc_speed(-forward + right);
|
||||
drive_pwm.set_config(&c);
|
||||
},
|
||||
ControlPacket::Fire => {
|
||||
|
|
@ -264,6 +267,9 @@ async fn main(spawner: Spawner) {
|
|||
ControlPacket::FireOverride(speed) => {
|
||||
cam_state.replace(CamState::Override(speed));
|
||||
},
|
||||
ControlPacket::SetAuto(state) => {
|
||||
auto_status.set_level(Level::from(state));
|
||||
},
|
||||
ControlPacket::Arm(enable) => {
|
||||
if enable {
|
||||
cam_state.replace(CamState::Charging);
|
||||
|
|
@ -282,16 +288,33 @@ async fn main(spawner: Spawner) {
|
|||
};
|
||||
|
||||
if let Ok(telem) = postcard::to_slice(&telem, &mut telem_buf) {
|
||||
if let Err(e) = socket.write_all(&u32::to_be_bytes(telem.len() as u32)).await {
|
||||
warn!("write error: {:?}", e);
|
||||
break;
|
||||
}
|
||||
if let Err(e) = socket.write_all(&telem).await {
|
||||
warn!("write error: {:?}", e);
|
||||
break;
|
||||
}
|
||||
//if let Err(e) = socket.flush().await {
|
||||
// warn!("write error: {:?}", e);
|
||||
// break;
|
||||
//}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 0 to 1
|
||||
fn calc_speed_mosfet(speed: f32) -> u16 {
|
||||
let speed = speed.clamp(0., 1.);
|
||||
|
||||
let speed = speed * u16::MAX as f32;
|
||||
|
||||
speed as u16
|
||||
}
|
||||
|
||||
/// -1 to 1
|
||||
fn calc_speed(speed: f32) -> u16 {
|
||||
const COUNTS_PER_MS: f32 = 3125.;
|
||||
|
|
@ -358,8 +381,8 @@ fn handle_cam(cam: &mut Pwm, state: &mut CamState, limit: &Input) {
|
|||
|
||||
|
||||
let mut f: pwm::Config = Default::default();
|
||||
f.divider = 40.into();
|
||||
f.top = 62500; // 20ms
|
||||
f.compare_b = calc_speed(-speed); // 1.5ms
|
||||
f.divider = 1.into();
|
||||
f.top = u16::MAX;
|
||||
f.compare_b = calc_speed_mosfet(speed);
|
||||
cam.set_config(&f);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -18,10 +18,11 @@ use crate::CHANNEL;
|
|||
pub struct SensorHardware {
|
||||
pub bus_tof: I2c<'static, I2C1, Async>,
|
||||
pub bus_gyro: I2c<'static, I2C0, Async>,
|
||||
pub tof2enable: Output<'static>,
|
||||
pub tof_l_enable: Output<'static>,
|
||||
pub tof_r_enable: Output<'static>,
|
||||
}
|
||||
|
||||
pub fn sensor_manager(mut hardware: SensorHardware) -> ! {
|
||||
pub fn sensor_manager(hardware: SensorHardware) -> ! {
|
||||
let mut executor = Executor::new();
|
||||
let executor: &'static mut Executor = unsafe{transmute(&mut executor)};
|
||||
executor.run(move |spawner| {
|
||||
|
|
@ -33,8 +34,7 @@ static POLL_STATE: AtomicU8 = AtomicU8::new(PollState::None as u8);
|
|||
#[repr(u8)]
|
||||
enum PollState {
|
||||
None,
|
||||
Tof1,
|
||||
Tof2,
|
||||
Tof,
|
||||
Gyro,
|
||||
}
|
||||
|
||||
|
|
@ -43,57 +43,66 @@ async fn init_sensors(mut hardware: SensorHardware) {
|
|||
let bus = RefCell::new(hardware.bus_tof);
|
||||
|
||||
Timer::after_millis(1).await;
|
||||
let mut tof = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).unwrap();
|
||||
tof.set_address(0x32).unwrap();
|
||||
hardware.tof2enable.set_high();
|
||||
Timer::after_micros(1200).await; // DS11555 3.2
|
||||
let mut tof2 = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).unwrap();
|
||||
let mut tof_s = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).ok();
|
||||
if let Some(ref mut tof_s) = tof_s {
|
||||
tof_s.set_address(0x32).unwrap();
|
||||
}
|
||||
|
||||
let mut gyro = Mpu6050::new_with_addr(hardware.bus_gyro,0x68);
|
||||
gyro.init(&mut Delay).unwrap();
|
||||
gyro.set_gyro_range(mpu6050::device::GyroRange::D2000).unwrap();
|
||||
gyro.set_accel_range(mpu6050::device::AccelRange::G16).unwrap();
|
||||
hardware.tof_l_enable.set_high();
|
||||
Timer::after_micros(1200).await; // DS11555 3.2
|
||||
let mut tof_l = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).ok();
|
||||
if let Some(ref mut tof_l) = tof_l {
|
||||
tof_l.set_address(0x33).unwrap();
|
||||
}
|
||||
|
||||
hardware.tof_r_enable.set_high();
|
||||
Timer::after_micros(1200).await; // DS11555 3.2
|
||||
let mut tof_r = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus)).ok();
|
||||
if let Some(ref mut tof_r) = tof_r {
|
||||
tof_r.set_address(0x34).unwrap();
|
||||
}
|
||||
|
||||
//let mut gyro = Mpu6050::new_with_addr(hardware.bus_gyro,0x68);
|
||||
//gyro.init(&mut Delay).unwrap();
|
||||
//gyro.set_gyro_range(mpu6050::device::GyroRange::D2000).unwrap();
|
||||
//gyro.set_accel_range(mpu6050::device::AccelRange::G16).unwrap();
|
||||
|
||||
|
||||
let mut data = SensorData {
|
||||
tof1: None,
|
||||
tof2: None,
|
||||
tof_l: None,
|
||||
tof_r: None,
|
||||
tof_s: None,
|
||||
gyro: None,
|
||||
accel: None,
|
||||
};
|
||||
let mut updated_ts = Instant::now();
|
||||
|
||||
loop {
|
||||
let mut updated = false;
|
||||
POLL_STATE.store(PollState::Tof1 as u8, Ordering::SeqCst);
|
||||
if let Ok(dist) = tof.read_range_single_millimeters_blocking() {
|
||||
info!("dist1: {dist}mm");
|
||||
data.tof1 = Some(dist);
|
||||
updated = true;
|
||||
}
|
||||
Timer::after_millis(3).await;
|
||||
POLL_STATE.store(PollState::Tof2 as u8, Ordering::SeqCst);
|
||||
if let Ok(dist) = tof2.read_range_single_millimeters_blocking() {
|
||||
info!("dist2: {dist}mm");
|
||||
data.tof2 = Some(dist);
|
||||
updated = true;
|
||||
}
|
||||
Timer::after_millis(3).await;
|
||||
POLL_STATE.store(PollState::Gyro as u8, Ordering::SeqCst);
|
||||
if let Ok(gyro) = gyro.get_gyro() {
|
||||
info!("rotation: {gyro}");
|
||||
data.gyro = Some(gyro);
|
||||
updated = true;
|
||||
}
|
||||
if let Ok(accel) = gyro.get_acc() {
|
||||
data.accel = Some(accel);
|
||||
updated = true;
|
||||
}
|
||||
if updated {
|
||||
updated_ts = Instant::now();
|
||||
POLL_STATE.store(PollState::Tof as u8, Ordering::SeqCst);
|
||||
for (tof, store) in
|
||||
[(&mut tof_s, &mut data.tof_s), (&mut tof_l, &mut data.tof_l), (&mut tof_r, &mut data.tof_r)] {
|
||||
if let Some(ref mut tof) = tof {
|
||||
if let Ok(dist) = tof.read_range_single_millimeters_blocking() {
|
||||
*store = Some(dist);
|
||||
} else {
|
||||
*store = None;
|
||||
}
|
||||
Timer::after_millis(3).await;
|
||||
}
|
||||
}
|
||||
|
||||
//POLL_STATE.store(PollState::Gyro as u8, Ordering::SeqCst);
|
||||
//if let Ok(gyro) = gyro.get_gyro() {
|
||||
// info!("rotation: {gyro}");
|
||||
// data.gyro = Some(gyro);
|
||||
// updated = true;
|
||||
//}
|
||||
//if let Ok(accel) = gyro.get_acc() {
|
||||
// data.accel = Some(accel);
|
||||
// updated = true;
|
||||
//}
|
||||
POLL_STATE.store(PollState::None as u8, Ordering::SeqCst);
|
||||
|
||||
CHANNEL.send((data.clone(), updated_ts.clone())).await;
|
||||
info!("sensors: {data:?}");
|
||||
CHANNEL.send((data.clone(), Instant::now())).await;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -46,7 +46,7 @@ fn main() -> Result<()> {
|
|||
//let latest_telem = Arc::new(RwLock::new(None));
|
||||
|
||||
let control = executor.block_on(async {
|
||||
let cruisecontrol = TcpStream::connect("169.254.1.1:1234").await?;
|
||||
let cruisecontrol = TcpStream::connect("192.168.1.2:1234").await?;
|
||||
println!("connected");
|
||||
cruisecontrol.set_nodelay(true)?;
|
||||
let (telem, control) = cruisecontrol.into_split();
|
||||
|
|
|
|||
Loading…
Reference in a new issue