triple tof init
This commit is contained in:
parent
57522c26cc
commit
782fd891a1
GPG key ID:
23F9463ECB67FE8C
5 changed files with 385 additions and 37 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"]}
|
||||
52
common/src/lib.rs
Normal file
52
common/src/lib.rs
Normal file
|
|
@ -0,0 +1,52 @@
|
|||
#![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),
|
||||
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 auto_status = Output::new(p.PIN_2, Level::High);
|
||||
|
||||
let mut d: pwm::Config = Default::default();
|
||||
d.divider = 40.into();
|
||||
|
|
@ -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(
|
||||
|
|
|
|||
|
|
@ -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,9 @@ static POLL_STATE: AtomicU8 = AtomicU8::new(PollState::None as u8);
|
|||
#[repr(u8)]
|
||||
enum PollState {
|
||||
None,
|
||||
Tof1,
|
||||
Tof2,
|
||||
TofL,
|
||||
TofR,
|
||||
TofS,
|
||||
Gyro,
|
||||
}
|
||||
|
||||
|
|
@ -43,21 +45,29 @@ 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)).unwrap();
|
||||
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)).unwrap();
|
||||
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)).unwrap();
|
||||
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,
|
||||
};
|
||||
|
|
@ -65,35 +75,43 @@ async fn init_sensors(mut hardware: SensorHardware) {
|
|||
|
||||
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);
|
||||
POLL_STATE.store(PollState::TofS as u8, Ordering::SeqCst);
|
||||
if let Ok(dist) = tof_s.read_range_single_millimeters_blocking() {
|
||||
data.tof_s = 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);
|
||||
|
||||
POLL_STATE.store(PollState::TofL as u8, Ordering::SeqCst);
|
||||
if let Ok(dist) = tof_l.read_range_single_millimeters_blocking() {
|
||||
data.tof_l = 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);
|
||||
|
||||
POLL_STATE.store(PollState::TofR as u8, Ordering::SeqCst);
|
||||
if let Ok(dist) = tof_r.read_range_single_millimeters_blocking() {
|
||||
data.tof_r = 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::None as u8, Ordering::SeqCst);
|
||||
|
||||
CHANNEL.send((data.clone(), updated_ts.clone())).await;
|
||||
info!("sensors: {data:?}");
|
||||
//CHANNEL.send((data.clone(), updated_ts.clone())).await;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue