From c21b033c4807d2d3ec29968fa68249aa779d6880 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Tue, 7 Jan 2025 20:48:50 -0500
Subject: [PATCH] distance sensor

---
 controller/Cargo.lock  | 11 +++++++++++
 controller/Cargo.toml  |  1 +
 controller/src/main.rs | 25 +++++++++++++++++++------
 3 files changed, 31 insertions(+), 6 deletions(-)

diff --git a/controller/Cargo.lock b/controller/Cargo.lock
index 72f4d51..13bdae2 100644
--- a/controller/Cargo.lock
+++ b/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"
diff --git a/controller/Cargo.toml b/controller/Cargo.toml
index 42651f6..5eba3f7 100644
--- a/controller/Cargo.toml
+++ b/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]
diff --git a/controller/src/main.rs b/controller/src/main.rs
index f16bb1f..6313cbb 100644
--- a/controller/src/main.rs
+++ b/controller/src/main.rs
@@ -8,6 +8,7 @@
 mod vl53l0;
 
 use core::array;
+use core::borrow::BorrowMut;
 use core::fmt::Formatter;
 use core::panic::PanicInfo;
 use core::str::from_utf8;
@@ -40,6 +41,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 +89,18 @@ 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 mut tof = vl53l0x::VL53L0x::new(bus1).unwrap();
+
+    tof.start_continuous(0).unwrap();
 
     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 +111,13 @@ 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_mm() {
+            info!("dist: {dist}mm");
+        }
+        Timer::after_millis(23).await;
     }
 
     let fw = include_bytes!("../../cyw43-firmware/43439A0.bin");