two distance sensors

controller/Cargo.lock

@@ -318,6 +318,7 @@ dependencies = [
  "st7789",
  "static_cell",
  "usbd-hid",
+ "vl53l0x",
 ]
 
 [[package]]
@@ -2397,6 +2398,16 @@ version = "0.9.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a"
 
+[[package]]
+name = "vl53l0x"
+version = "1.0.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e943add0e260b9f7030223a74bf30d8bb020747a4a3472c281bab1562046f078"
+dependencies = [
+ "embedded-hal 1.0.0",
+ "nb 0.1.3",
+]
+
 [[package]]
 name = "void"
 version = "1.0.2"

controller/Cargo.toml

@@ -60,6 +60,7 @@ embedded-sdmmc = "0.7.0"
 bt-hci = { version = "0.1.0", default-features = false, features = ["defmt"] }
 hex = { version = "0.4.3", default-features=false }
 mpu6050 = { git = "https://git.ank.dev/ank/mpu6050" }
+vl53l0x = "1.0.1"
 
 
 [profile.release]

controller/src/main.rs

@@ -8,6 +8,8 @@
 mod vl53l0;
 
 use core::array;
+use core::borrow::BorrowMut;
+use core::cell::RefCell;
 use core::fmt::Formatter;
 use core::panic::PanicInfo;
 use core::str::from_utf8;
@@ -16,6 +18,7 @@ use bt_hci::cmd::info;
 use cyw43_pio::PioSpi;
 use embassy_rp::i2c::{Async, I2c};
 use embassy_rp::pwm::{self, Pwm};
+use embedded_hal_bus::i2c::RefCellDevice;
 use fixed::FixedU16;
 use log::*;
 //use embassy_rp::i2c::InterruptHandler;
@@ -40,6 +43,7 @@ use vl53l0::RegAddr::*;
 bind_interrupts!(struct Irqs {
     PIO0_IRQ_0 => InterruptHandler<PIO0>;
     I2C0_IRQ => embassy_rp::i2c::InterruptHandler<embassy_rp::peripherals::I2C0>;
+    I2C1_IRQ => embassy_rp::i2c::InterruptHandler<embassy_rp::peripherals::I2C1>;
     USBCTRL_IRQ => embassy_rp::usb::InterruptHandler<USB>;
 });
 
@@ -87,10 +91,31 @@ async fn main(spawner: Spawner) {
     let sda = p.PIN_20;
     let scl = p.PIN_21;
     let config = embassy_rp::i2c::Config::default();
-    let mut bus = embassy_rp::i2c::I2c::new_async(p.I2C0, scl, sda, Irqs, config);
+    let bus = embassy_rp::i2c::I2c::new_async(p.I2C0, scl, sda, Irqs, config);
+
+    let sda = p.PIN_26;
+    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 bus1 = RefCell::new(bus1);
+
+    info!("made RefCell");
+
+    let mut tof2on = Output::new(p.PIN_16, Level::Low);
+    info!("2");
+    Timer::after_millis(1).await;
+    let mut tof = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus1)).unwrap();
+    info!("3");
+    tof.set_address(0x32).unwrap();
+    info!("4");
+    tof2on.set_high();
+    Timer::after_millis(13).await;
+    info!("5");
+    let mut tof2 = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus1)).unwrap();
+    info!("6");
+
 
     let mut gyro = Mpu6050::new_with_addr(bus,0x68);
-    let mut gyro2 = Mpu6050::new_with_addr(bus,0x69);
 
     Timer::after_millis(1300).await;
     gyro.init(&mut Delay).unwrap();
@@ -101,10 +126,17 @@ async fn main(spawner: Spawner) {
     Timer::after_millis(1300).await;
 
     loop {
-        info!("pos: {}", gyro.get_gyro().unwrap().x);
-        info!("jerk: {}", gyro.get_acc().unwrap().x);
-        info!("temp: {}", gyro.get_temp().unwrap() * (9./5.) + 32.);
-        Timer::after_millis(13).await;
+        //info!("pos: {}", gyro.get_gyro().unwrap().x);
+        //info!("jerk: {}", gyro.get_acc().unwrap().x);
+        //info!("temp: {}", gyro.get_temp().unwrap() * (9./5.) + 32.);
+        if let Ok(dist) = tof.read_range_single_millimeters_blocking() {
+            info!("dist1: {dist}mm");
+        }
+        Timer::after_millis(3).await;
+        if let Ok(dist) = tof2.read_range_single_millimeters_blocking() {
+            info!("dist2: {dist}mm");
+        }
+        Timer::after_millis(23).await;
     }
 
     let fw = include_bytes!("../../cyw43-firmware/43439A0.bin");