diff --git a/common/Cargo.lock b/common/Cargo.lock new file mode 100644 index 0000000..a8538e1 --- /dev/null +++ b/common/Cargo.lock @@ -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" diff --git a/common/Cargo.toml b/common/Cargo.toml new file mode 100644 index 0000000..9a5ff0a --- /dev/null +++ b/common/Cargo.toml @@ -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"]} diff --git a/common/src/lib.rs b/common/src/lib.rs new file mode 100644 index 0000000..26bbe61 --- /dev/null +++ b/common/src/lib.rs @@ -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, + /// right distance, mm + pub tof_r: Option, + /// left side distance, mm + pub tof_s: Option, + /// acceleration, rad/s + pub gyro: Option>, + /// acceleration, g + pub accel: Option>, +} + +#[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, +} diff --git a/controller/src/main.rs b/controller/src/main.rs index b01839d..1b6d800 100644 --- a/controller/src/main.rs +++ b/controller/src/main.rs @@ -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( diff --git a/controller/src/sensor_manager.rs b/controller/src/sensor_manager.rs index 4da621b..329ccd6 100644 --- a/controller/src/sensor_manager.rs +++ b/controller/src/sensor_manager.rs @@ -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; } }