Add time of flight controller firmware
The tof controller uses 2 lidar sensors to estimate the center of the held cone
in the end effector and communicates it back to the roborio over pwm so that
we can compensate.
This change also vendors in ST's library for the VL53L1X and adds a
platform specific i2c implementation for the raspberry pi pico.
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: Id05c3b539df9c5a1ee4591e0abaf0a07df85674a
diff --git a/WORKSPACE b/WORKSPACE
index 64f2dee..a4d4406 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -1392,3 +1392,15 @@
url =
"https://www.frc971.org/Build-Dependencies/foxglove-d6b00825.tar.gz",
)
+
+#
+# https://www.st.com/en/embedded-software/stsw-img009.html#overview
+http_archive(
+ name = "vl53l1x_ultra_lite_driver_api",
+ build_file = "//third_party/vl53l1x:vl53l1x.BUILD",
+ patch_args = ["-p1"],
+ patches = ["//third_party/vl53l1x:vl53l1x.patch"],
+ sha256 = "06a66254ab7a8b061f93ff0f65abb6088c3ea50335475bb6ac11087beb65d174",
+ strip_prefix = "en.STSW-IMG009_v3.5.2/API",
+ url = "https://www.frc971.org/Build-Dependencies/en.STSW-IMG009.zip",
+)
diff --git a/third_party/pico-sdk/BUILD b/third_party/pico-sdk/BUILD
index ef2bf81..3498b9a 100644
--- a/third_party/pico-sdk/BUILD
+++ b/third_party/pico-sdk/BUILD
@@ -35,6 +35,7 @@
"src/rp2_common/hardware_divider/divider.S",
"src/rp2_common/hardware_dma/dma.c",
"src/rp2_common/hardware_flash/flash.c",
+ "src/rp2_common/hardware_i2c/i2c.c",
"src/rp2_common/hardware_gpio/gpio.c",
"src/rp2_common/hardware_irq/irq.c",
"src/rp2_common/hardware_irq/irq_handler_chain.S",
@@ -194,6 +195,7 @@
"src/rp2_common/hardware_divider/include/hardware/divider_helper.S",
"src/rp2_common/hardware_dma/include/hardware/dma.h",
"src/rp2_common/hardware_flash/include/hardware/flash.h",
+ "src/rp2_common/hardware_i2c/include/hardware/i2c.h",
"src/rp2_common/hardware_gpio/include/hardware/gpio.h",
"src/rp2_common/hardware_irq/include/hardware/irq.h",
"src/rp2_common/hardware_pio/include/hardware/pio.h",
@@ -296,6 +298,7 @@
"src/rp2_common/hardware_divider/include",
"src/rp2_common/hardware_dma/include",
"src/rp2_common/hardware_flash/include",
+ "src/rp2_common/hardware_i2c/include",
"src/rp2_common/hardware_gpio/include",
"src/rp2_common/hardware_irq/include",
"src/rp2_common/hardware_pio/include",
diff --git a/third_party/vl53l1x/BUILD b/third_party/vl53l1x/BUILD
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/third_party/vl53l1x/BUILD
diff --git a/third_party/vl53l1x/vl53l1x.BUILD b/third_party/vl53l1x/vl53l1x.BUILD
new file mode 100644
index 0000000..13ea7fd
--- /dev/null
+++ b/third_party/vl53l1x/vl53l1x.BUILD
@@ -0,0 +1,26 @@
+
+cc_library(
+ name = "VL53L1X",
+ srcs = [
+ "core/VL53L1X_api.c",
+ "core/VL53L1X_calibration.c",
+ "platform/vl53l1_platform.c",
+ ],
+ hdrs = [
+ "core/VL53L1X_api.h",
+ "core/VL53L1X_calibration.h",
+ "platform/vl53l1_platform.h",
+ "platform/vl53l1_types.h",
+ ],
+ target_compatible_with = [
+ "@platforms//os:none",
+ "@//tools/platforms/hardware:cortex_m0plus",
+ ],
+ copts = [
+ "-Wno-unused-parameter",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "@//third_party/pico-sdk",
+ ],
+)
diff --git a/third_party/vl53l1x/vl53l1x.patch b/third_party/vl53l1x/vl53l1x.patch
new file mode 100644
index 0000000..d05a42f
--- /dev/null
+++ b/third_party/vl53l1x/vl53l1x.patch
@@ -0,0 +1,310 @@
+diff --git a/core/VL53L1X_api.c b/core/VL53L1X_api.c
+index 572040d..7cdb719 100644
+--- a/core/VL53L1X_api.c
++++ b/core/VL53L1X_api.c
+@@ -62,7 +62,7 @@
+ * @brief Functions implementation
+ */
+
+-#include "VL53L1X_api.h"
++#include "core/VL53L1X_api.h"
+ #include <string.h>
+
+ #if 0
+@@ -665,7 +665,7 @@ VL53L1X_ERROR VL53L1X_GetOffset(uint16_t dev, int16_t *offset)
+ Temp = Temp>>5;
+ *offset = (int16_t)(Temp);
+
+- if(*offset > 1024)
++ if(*offset > 1024)
+ {
+ *offset = *offset - 2048;
+ }
+diff --git a/core/VL53L1X_api.h b/core/VL53L1X_api.h
+index fc93e56..790ffcb 100644
+--- a/core/VL53L1X_api.h
++++ b/core/VL53L1X_api.h
+@@ -68,7 +68,11 @@
+ #ifndef _API_H_
+ #define _API_H_
+
+-#include "vl53l1_platform.h"
++#include "platform/vl53l1_platform.h"
++
++#ifdef __cplusplus
++extern "C" {
++#endif
+
+ #define VL53L1X_IMPLEMENTATION_VER_MAJOR 3
+ #define VL53L1X_IMPLEMENTATION_VER_MINOR 5
+@@ -390,4 +394,8 @@ VL53L1X_ERROR VL53L1X_GetSigmaThreshold(uint16_t dev, uint16_t *signal);
+ */
+ VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(uint16_t dev);
+
++#ifdef __cplusplus
++}
++#endif
++
+ #endif
+diff --git a/core/VL53L1X_calibration.c b/core/VL53L1X_calibration.c
+index 0c58fb1..59381da 100644
+--- a/core/VL53L1X_calibration.c
++++ b/core/VL53L1X_calibration.c
+@@ -63,8 +63,8 @@
+ * @file vl53l1x_calibration.c
+ * @brief Calibration functions implementation
+ */
+-#include "VL53L1X_api.h"
+-#include "VL53L1X_calibration.h"
++#include "core/VL53L1X_api.h"
++#include "core/VL53L1X_calibration.h"
+
+ #define ALGO__PART_TO_PART_RANGE_OFFSET_MM 0x001E
+ #define MM_CONFIG__INNER_OFFSET_MM 0x0020
+diff --git a/core/VL53L1X_calibration.h b/core/VL53L1X_calibration.h
+index c057722..eaa405a 100644
+--- a/core/VL53L1X_calibration.h
++++ b/core/VL53L1X_calibration.h
+@@ -67,6 +67,12 @@
+ #ifndef _CALIBRATION_H_
+ #define _CALIBRATION_H_
+
++#include <stdint.h>
++
++#ifdef __cplusplus
++extern "C" {
++#endif
++
+ /**
+ * @brief This function performs the offset calibration.\n
+ * The function returns the offset value found and programs the offset compensation into the device.
+@@ -90,4 +96,8 @@ int8_t VL53L1X_CalibrateOffset(uint16_t dev, uint16_t TargetDistInMm, int16_t *o
+ */
+ int8_t VL53L1X_CalibrateXtalk(uint16_t dev, uint16_t TargetDistInMm, uint16_t *xtalk);
+
++#ifdef __cplusplus
++}
++#endif
++
+ #endif
+diff --git a/platform/vl53l1_platform.c b/platform/vl53l1_platform.c
+index 4d17113..0cd0620 100644
+--- a/platform/vl53l1_platform.c
++++ b/platform/vl53l1_platform.c
+@@ -1,75 +1,144 @@
+
+-/*
+-* This file is part of VL53L1 Platform
+-*
+-* Copyright (c) 2016, STMicroelectronics - All Rights Reserved
+-*
+-* License terms: BSD 3-clause "New" or "Revised" License.
+-*
+-* Redistribution and use in source and binary forms, with or without
+-* modification, are permitted provided that the following conditions are met:
+-*
+-* 1. Redistributions of source code must retain the above copyright notice, this
+-* list of conditions and the following disclaimer.
+-*
+-* 2. Redistributions in binary form must reproduce the above copyright notice,
+-* this list of conditions and the following disclaimer in the documentation
+-* and/or other materials provided with the distribution.
+-*
+-* 3. Neither the name of the copyright holder nor the names of its contributors
+-* may be used to endorse or promote products derived from this software
+-* without specific prior written permission.
+-*
+-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+-* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+-* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+-* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+-* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+-* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+-* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+-* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+-* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+-*
++/*
++* This file is part of VL53L1 Platform
++*
++* Copyright (c) 2016, STMicroelectronics - All Rights Reserved
++*
++* License terms: BSD 3-clause "New" or "Revised" License.
++*
++* Redistribution and use in source and binary forms, with or without
++* modification, are permitted provided that the following conditions are met:
++*
++* 1. Redistributions of source code must retain the above copyright notice, this
++* list of conditions and the following disclaimer.
++*
++* 2. Redistributions in binary form must reproduce the above copyright notice,
++* this list of conditions and the following disclaimer in the documentation
++* and/or other materials provided with the distribution.
++*
++* 3. Neither the name of the copyright holder nor the names of its contributors
++* may be used to endorse or promote products derived from this software
++* without specific prior written permission.
++*
++* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
++* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
++* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
++* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
++* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
++* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
++* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
++* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
++* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
++* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
++*
+ */
+
+-#include "vl53l1_platform.h"
++#include "platform/vl53l1_platform.h"
++#include <stdint.h>
+ #include <string.h>
+ #include <time.h>
+ #include <math.h>
+
+-int8_t VL53L1_WriteMulti( uint16_t dev, uint16_t index, uint8_t *pdata, uint32_t count) {
+- return 0; // to be implemented
++#include "hardware/i2c.h"
++#include "pico/stdlib.h"
++
++static uint8_t
++ buffer[VL53L1X_I2C_BUF_SIZE + 2]; // R/W buffer for all transactions
++
++/* ----- Static helper functions ----- */
++
++// Read len bytes of data from i2c device with address dev into buff
++// Expects register address to already be configured
++// Returns 0 on success, -1 on failure
++static int8_t Pico_I2CRead(i2c_inst_t *i2c_dev, uint16_t addr, uint8_t *buff,
++ uint8_t len) {
++ return (i2c_read_blocking(i2c_dev, addr, buff, len, false) == len) - 1;
++}
++
++// Write len bytes of data to i2c device with address dev from buff
++// Returns 0 on success, -1 on failure
++static int8_t Pico_I2CWrite(i2c_inst_t *i2c_dev, uint16_t addr, uint8_t *buff,
++ uint8_t len) {
++ return (i2c_write_blocking(i2c_dev, addr, buff, len, false) == len) - 1;
+ }
+
+-int8_t VL53L1_ReadMulti(uint16_t dev, uint16_t index, uint8_t *pdata, uint32_t count){
+- return 0; // to be implemented
++int8_t VL53L1_WriteMulti(uint16_t dev, uint16_t index, uint8_t *pdata,
++ uint32_t count) {
++ if (count >= VL53L1X_I2C_BUF_SIZE) {
++ return -1;
++ }
++
++ buffer[0] = 0xFF & (index >> 8);
++ buffer[1] = 0xFF & index;
++ memcpy(&buffer[2], pdata, count);
++
++ i2c_inst_t *i2c_dev = i2c0;
++ if (dev == 1) {
++ i2c_dev = i2c0;
++ } else if (dev == 2) {
++ i2c_dev = i2c1;
++ }
++
++ return Pico_I2CWrite(i2c_dev, VL53L1X_I2C_DEV_ADDR, buffer, (count + 2));
++}
++
++int8_t VL53L1_ReadMulti(uint16_t dev, uint16_t index, uint8_t *pdata,
++ uint32_t count) {
++ if (count >= VL53L1X_I2C_BUF_SIZE) {
++ return -1;
++ }
++
++ buffer[0] = 0xFF & (index >> 8);
++ buffer[1] = 0xFF & index;
++
++ i2c_inst_t *i2c_dev = i2c0;
++ if (dev == 1) {
++ i2c_dev = i2c0;
++ } else if (dev == 2) {
++ i2c_dev = i2c1;
++ }
++
++ int8_t result = Pico_I2CWrite(i2c_dev, VL53L1X_I2C_DEV_ADDR, buffer, 2);
++
++ if (result == 0) {
++ pdata[0] = index;
++ result = Pico_I2CRead(i2c_dev, VL53L1X_I2C_DEV_ADDR, pdata, count);
++ }
++
++ return result;
+ }
+
+ int8_t VL53L1_WrByte(uint16_t dev, uint16_t index, uint8_t data) {
+- return 0; // to be implemented
++ return VL53L1_WriteMulti(dev, index, &data, 1);
+ }
+
+ int8_t VL53L1_WrWord(uint16_t dev, uint16_t index, uint16_t data) {
+- return 0; // to be implemented
++ data = __htons(data);
++ return VL53L1_WriteMulti(dev, index, (uint8_t *)&data, 2);
+ }
+
+ int8_t VL53L1_WrDWord(uint16_t dev, uint16_t index, uint32_t data) {
+- return 0; // to be implemented
++ data = __htonl(data);
++ return VL53L1_WriteMulti(dev, index, (uint8_t *)&data, 4);
+ }
+
+ int8_t VL53L1_RdByte(uint16_t dev, uint16_t index, uint8_t *data) {
+- return 0; // to be implemented
++ return VL53L1_ReadMulti(dev, index, data, 1);
+ }
+
+ int8_t VL53L1_RdWord(uint16_t dev, uint16_t index, uint16_t *data) {
+- return 0; // to be implemented
++ int8_t result = VL53L1_ReadMulti(dev, index, (uint8_t *)data, 2);
++ *data = __ntohs(*data);
++ return result;
+ }
+
+ int8_t VL53L1_RdDWord(uint16_t dev, uint16_t index, uint32_t *data) {
+- return 0; // to be implemented
++ int8_t result = VL53L1_ReadMulti(dev, index, (uint8_t *)data, 4);
++ *data = __ntohl(*data);
++ return result;
+ }
+
+-int8_t VL53L1_WaitMs(uint16_t dev, int32_t wait_ms){
+- return 0; // to be implemented
++int8_t VL53L1_WaitMs(uint16_t dev, int32_t wait_ms) {
++ sleep_ms(wait_ms);
++ return 0;
+ }
+diff --git a/platform/vl53l1_platform.h b/platform/vl53l1_platform.h
+index 75bc4c2..e657ba6 100644
+--- a/platform/vl53l1_platform.h
++++ b/platform/vl53l1_platform.h
+@@ -2,17 +2,20 @@
+ * @file vl53l1_platform.h
+ * @brief Those platform functions are platform dependent and have to be implemented by the user
+ */
+-
++
+ #ifndef _VL53L1_PLATFORM_H_
+ #define _VL53L1_PLATFORM_H_
+
+-#include "vl53l1_types.h"
++#include "platform/vl53l1_types.h"
+
+ #ifdef __cplusplus
+ extern "C"
+ {
+ #endif
+
++#define VL53L1X_I2C_BUF_SIZE 256
++#define VL53L1X_I2C_DEV_ADDR 0x29
++
+ typedef struct {
+ uint32_t dummy;
+ } VL53L1_Dev_t;
diff --git a/y2023/tof_controller/BUILD b/y2023/tof_controller/BUILD
new file mode 100644
index 0000000..e3045b0
--- /dev/null
+++ b/y2023/tof_controller/BUILD
@@ -0,0 +1,27 @@
+load("//third_party/pico-sdk:hex.bzl", "uf2_from_elf")
+
+cc_binary(
+ name = "tof_controller.elf",
+ srcs = [
+ "tof_controller.cc",
+ "//third_party/pico-sdk/src/rp2_common/boot_stage2:bs2_default_padded_checksummed.S",
+ ],
+ additional_linker_inputs = [
+ "//third_party/pico-sdk:src/rp2_common/pico_standard_link/memmap_default.ld",
+ ],
+ linkopts = [
+ "-Wl,--script=third_party/pico-sdk/src/rp2_common/pico_standard_link/memmap_default.ld",
+ ],
+ target_compatible_with = [
+ "@platforms//os:none",
+ "//tools/platforms/hardware:cortex_m0plus",
+ ],
+ deps = [
+ "//third_party/pico-sdk",
+ "@vl53l1x_ultra_lite_driver_api//:VL53L1X",
+ ],
+)
+
+uf2_from_elf(
+ name = "tof_controller",
+)
diff --git a/y2023/tof_controller/CMakeLists.txt b/y2023/tof_controller/CMakeLists.txt
new file mode 100644
index 0000000..58db271
--- /dev/null
+++ b/y2023/tof_controller/CMakeLists.txt
@@ -0,0 +1,50 @@
+
+cmake_minimum_required(VERSION 3.13)
+
+# initialize the SDK based on PICO_SDK_PATH
+# note: this must happen before project()
+
+#set(PICO_SDK_FETCH_FROM_GIT 1)
+set(PICO_SDK_PATH ../../../third_party/pico-sdk)
+set(PICO_TOOLCHAIN_PATH ../../external/gcc_arm_none_eabi/bin)
+
+include(../../third_party/pico-sdk/external/pico_sdk_import.cmake)
+
+project(tof_controller)
+
+# the root of the repository
+include_directories(../../)
+
+# initialize the Raspberry Pi Pico SDK
+pico_sdk_init()
+
+set(CMAKE_CXX_STANDARD 17)
+
+# rest of your project
+
+include_directories(../../external/vl53l1x_ultra_lite_driver_api)
+add_library(VL53L1X
+ ../../external/vl53l1x_ultra_lite_driver_api/platform/vl53l1_platform.h
+ ../../external/vl53l1x_ultra_lite_driver_api/platform/vl53l1_platform.c
+ ../../external/vl53l1x_ultra_lite_driver_api/platform/vl53l1_types.h
+ ../../external/vl53l1x_ultra_lite_driver_api/core/VL53L1X_api.c
+ ../../external/vl53l1x_ultra_lite_driver_api/core/VL53L1X_api.h
+ ../../external/vl53l1x_ultra_lite_driver_api/core/VL53L1X_calibration.h
+ ../../external/vl53l1x_ultra_lite_driver_api/core/VL53L1X_calibration.c)
+
+add_executable(tof_controller
+ tof_controller.cc
+ )
+
+# pull in common dependencies
+target_link_libraries(tof_controller VL53L1X)
+target_link_libraries(VL53L1X pico_stdlib hardware_i2c hardware_pwm)
+target_link_libraries(tof_controller pico_stdlib hardware_i2c)
+
+# enable usb output, disable uart output
+pico_enable_stdio_usb(tof_controller 1)
+pico_enable_stdio_uart(tof_controller 0)
+
+# create map/bin/hex/uf2 file etc.
+pico_add_extra_outputs(tof_controller)
+
diff --git a/y2023/tof_controller/tof_controller.cc b/y2023/tof_controller/tof_controller.cc
new file mode 100644
index 0000000..645eb95
--- /dev/null
+++ b/y2023/tof_controller/tof_controller.cc
@@ -0,0 +1,504 @@
+#include <stdio.h>
+
+#include <algorithm>
+#include <limits>
+#include <cmath>
+
+#include "hardware/clocks.h"
+#include "hardware/i2c.h"
+#include "hardware/pwm.h"
+#include "hardware/watchdog.h"
+#include "pico/bootrom.h"
+#include "pico/stdlib.h"
+#include "core/VL53L1X_api.h"
+#include "core/VL53L1X_calibration.h"
+
+namespace y2023 {
+namespace tof_controller {
+
+static constexpr uint kI2CBaudrate = 100000;
+static constexpr uint16_t kExpectedSensorId = 0xEBAA;
+
+static constexpr uint16_t kTimingBudgetMs = 33; // 15 ms is fastest
+static constexpr double kWatchdogTimeoutMs = 2000;
+static constexpr int64_t kSensorBootTimeoutMs = 3;
+
+static constexpr double kSensorSeparation = 0.45;
+static constexpr int16_t kSensorOffsetMM = -22;
+
+/*
+// Cone base
+static constexpr double kMaxObstructionWidth = 0.22 + 0.07;
+// cone tip
+static constexpr double kMinObstructionWidth = 0.044 + 0.07;
+*/
+
+namespace sensor1 {
+static constexpr uint kPinSCL = 1;
+static constexpr uint kPinSDA = 0;
+
+static constexpr uint kPinStatusGreen = 3;
+static constexpr uint kPinStatusRed = 5;
+static constexpr uint kPinStatusPWM = 16;
+
+static constexpr uint kPinInterrupt = 8;
+static constexpr uint kPinShutdown = 9;
+
+static constexpr uint kPinOutput = 2;
+} // namespace sensor1
+
+namespace sensor2 {
+static constexpr uint kPinSCL = 26;
+static constexpr uint kPinSDA = 27;
+
+static constexpr uint kPinStatusGreen = 14;
+static constexpr uint kPinStatusRed = 13;
+static constexpr uint kPinStatusPWM = 17;
+
+static constexpr uint kPinInterrupt = 22;
+static constexpr uint kPinShutdown = 28;
+
+static constexpr uint kPinOutput = 4;
+} // namespace sensor2
+
+class SignalWriter {
+ public:
+ static constexpr double kScaledRangeLow = 0.1;
+ static constexpr double kScaledRangeHigh = 0.9;
+
+ // PWM counts to this before wrapping
+ static constexpr uint16_t kPWMTop = 62499;
+ static constexpr int kPWMFreqHz = 200;
+
+ SignalWriter(uint pin) : pin_(pin) {
+ gpio_init(pin);
+ gpio_set_function(pin, GPIO_FUNC_PWM);
+
+ uint slice_num = pwm_gpio_to_slice_num(pin);
+ pwm_set_enabled(slice_num, true);
+
+ // TODO(Ravago): Verify that this still works the same as the imu board
+
+ /* frequency_pwm = f_sys / ((TOP + 1) * (CSR_PHASE_CORRECT + 1) * (DIV_INT +
+ * DIV_FRAC / 16))
+ *
+ * f_sys = 125 mhz
+ * CSR_PHASE_CORRECT = 0; no phase correct
+ * TARGET_FREQ = 200 hz
+ *
+ * 125 mhz / x = 200 hz * 62500
+ *
+ * TOP = 62499
+ * DIV_INT = 10
+ */
+
+ float divisor = clock_get_hz(clk_sys) / (kPWMTop + 1) / kPWMFreqHz;
+
+ pwm_config cfg = pwm_get_default_config();
+ pwm_config_set_clkdiv_mode(&cfg, PWM_DIV_FREE_RUNNING);
+ pwm_config_set_clkdiv(&cfg, divisor);
+ pwm_config_set_wrap(&cfg, kPWMTop);
+
+ pwm_init(slice_num, &cfg, true);
+
+ Write();
+ }
+
+ void SetValue(double value) {
+ value_ = std::clamp(value, 0.0, 1.0);
+ Write();
+ }
+
+ void SetEnabled(bool enabled) {
+ enabled_ = enabled;
+ Write();
+ }
+
+ private:
+ void Write() {
+ double scaled_value =
+ (value_ * (kScaledRangeHigh - kScaledRangeLow) + kScaledRangeLow);
+
+ uint16_t level = scaled_value * kPWMTop;
+
+ if (!enabled_) {
+ level = 0;
+ }
+
+ pwm_set_gpio_level(pin_, level);
+ }
+
+ double value_ = 0.0;
+ uint pin_;
+ bool enabled_ = true;
+};
+
+class BrightnessLED {
+ public:
+ BrightnessLED(uint pin) : pin_(pin) {
+ gpio_init(pin);
+ gpio_set_function(pin, GPIO_FUNC_PWM);
+
+ uint slice_num = pwm_gpio_to_slice_num(pin);
+ pwm_set_enabled(slice_num, true);
+
+ Write();
+ }
+
+ void SetValue(double value) {
+ value_ = std::clamp(value, 0.0, 1.0);
+ Write();
+ }
+
+ private:
+ void Write() {
+ uint16_t level = value_ * std::numeric_limits<uint16_t>::max();
+ pwm_set_gpio_level(pin_, level);
+ }
+
+ double value_ = 0;
+ uint pin_;
+};
+
+class StatusLED {
+ public:
+ StatusLED(uint pin_green, uint pin_red)
+ : pin_green_(pin_green), pin_red_(pin_red) {
+ gpio_init(pin_green);
+ gpio_init(pin_red);
+ gpio_set_dir(pin_green, GPIO_OUT);
+ gpio_set_dir(pin_red, GPIO_OUT);
+
+ SetError();
+ }
+
+ void SetOk() { Set(true); }
+
+ void SetError() { Set(false); }
+
+ void Set(bool is_good) {
+ status_ok_ = is_good;
+ Write();
+ }
+
+ void Toggle() {
+ status_ok_ = !status_ok_;
+ Write();
+ }
+
+ bool get_status() { return status_ok_; }
+
+ private:
+ void Write() {
+ if (status_ok_) {
+ gpio_put(pin_green_, true);
+ gpio_put(pin_red_, false);
+ } else {
+ gpio_put(pin_green_, false);
+ gpio_put(pin_red_, true);
+ }
+ }
+
+ uint pin_green_, pin_red_;
+ bool status_ok_ = false;
+};
+
+class VL53L1X {
+ public:
+ VL53L1X(int id, StatusLED status_light, BrightnessLED brightness,
+ uint interrupt_pin, uint shutdown_pin)
+ : id_(id),
+ status_light_(status_light),
+ brightness_(brightness),
+ interrupt_pin_(interrupt_pin),
+ shutdown_pin_(shutdown_pin) {
+ gpio_init(interrupt_pin);
+ gpio_set_dir(interrupt_pin, GPIO_IN);
+
+ gpio_init(shutdown_pin);
+ gpio_set_dir(shutdown_pin, GPIO_OUT);
+ gpio_put(shutdown_pin, true);
+
+ InitSensor();
+ }
+
+ void WaitForData() {
+ uint8_t data_ready = false;
+ while (!data_ready) {
+ CheckOk(VL53L1X_CheckForDataReady(id_, &data_ready));
+
+ if (errors > 0) {
+ // don't block the main loop if we're in an errored state
+ break;
+ }
+
+ sleep_us(1);
+ }
+ }
+
+ VL53L1X_Result_t Read() {
+ VL53L1X_Result_t result;
+
+ CheckOk(VL53L1X_GetResult(id_, &result));
+
+ double meters = static_cast<double>(result.Distance) * 0.001;
+ brightness_.SetValue(meters / kSensorSeparation);
+
+ return result;
+ }
+
+ void Stop() {
+ CheckOk(VL53L1X_StopRanging(id_));
+
+ // StopRanging command is checked when the interrupt is cleared
+ CheckOk(VL53L1X_ClearInterrupt(id_));
+
+ // wait for it to actually turn off the light so it doesn't interfere with
+ // the other sensor
+ sleep_ms(3);
+ }
+
+ void Start() { CheckOk(VL53L1X_StartRanging(id_)); }
+
+ // Try to reinitialize the sensor if it is in a bad state (eg. lost power
+ // but is now reconnected)
+ void MaybeAttemptRecovery() {
+ if (errors > 0 && consecutive_errors == 0) {
+ // it's probably back
+ printf("Attempting to recover from an errored state on sensor %d.\n",
+ id_);
+ printf("Previously had %d total errors\n", errors);
+
+ // Reboot to put it in a known state.
+ gpio_put(shutdown_pin_, false);
+ sleep_ms(500);
+ gpio_put(shutdown_pin_, true);
+
+ InitSensor();
+ }
+ }
+
+ int errors = 0;
+ int consecutive_errors = 0;
+
+ private:
+ void InitSensor() {
+ printf("Waiting for sensor %d to boot.\n", id_);
+
+ absolute_time_t start_time = get_absolute_time();
+ uint8_t boot_state = 0;
+ while (boot_state == 0) {
+ CheckOk(VL53L1X_BootState(id_, &boot_state));
+
+ int64_t diff = absolute_time_diff_us(start_time, get_absolute_time());
+
+ if (diff > (kSensorBootTimeoutMs * 1000)) {
+ printf("Timed out after %lld us\n", diff);
+ return;
+ }
+
+ sleep_us(1);
+ }
+ printf("Bootstate: %d\n", boot_state);
+
+ int status = 0;
+
+ printf("Getting sensor id\n");
+ // Validate sensor model ID and Type
+ uint16_t sensor_id;
+ status += VL53L1X_GetSensorId(id_, &sensor_id);
+ if (sensor_id != kExpectedSensorId) { // Bad connection, wrong chip, etc
+ printf("Bad sensor id %x, continuing anyways\n", sensor_id);
+ status_light_.SetError();
+ }
+ printf("Got sensor id: %x\n", sensor_id);
+
+ // SensorInit will cause the processor to hang and hit the watchdog if the
+ // sensor is in a bad state
+ status += VL53L1X_SensorInit(id_);
+
+ status += VL53L1X_SetDistanceMode(id_, 1); // Set distance mode to short
+ status += VL53L1X_SetTimingBudgetInMs(id_, kTimingBudgetMs);
+ status += VL53L1X_SetInterMeasurementInMs(id_, kTimingBudgetMs);
+ status += VL53L1X_SetOffset(id_, kSensorOffsetMM);
+ status += VL53L1X_StartRanging(id_);
+
+ if (status == 0) {
+ printf("Successfully configured sensor %d\n", id_);
+
+ status_light_.SetOk();
+ errors = 0;
+ consecutive_errors = 0;
+ } else {
+ printf("Failed to configure sensor %d\n", id_);
+ }
+ }
+
+ void CheckOk(VL53L1X_ERROR error) {
+ if (error == 0) {
+ consecutive_errors = 0;
+ return;
+ }
+
+ consecutive_errors++;
+ errors++;
+
+ status_light_.SetError();
+ }
+
+ // id of the sensor ie. sensor 1 or sensor 2
+ int id_;
+ StatusLED status_light_;
+ BrightnessLED brightness_;
+ uint interrupt_pin_;
+ uint shutdown_pin_;
+};
+
+int main() {
+ stdio_init_all();
+
+ if (watchdog_caused_reboot()) {
+ printf("Rebooted by Watchdog!\n");
+ } else {
+ printf("Clean boot\n");
+ }
+
+ // Enable the watchdog, requiring the watchdog to be updated every 100ms or
+ // the chip will reboot second arg is pause on debug which means the watchdog
+ // will pause when stepping through code
+ watchdog_enable(kWatchdogTimeoutMs, 1);
+
+ i2c_init(i2c0, kI2CBaudrate);
+ gpio_set_function(sensor1::kPinSCL, GPIO_FUNC_I2C);
+ gpio_set_function(sensor1::kPinSDA, GPIO_FUNC_I2C);
+
+ i2c_init(i2c1, kI2CBaudrate);
+ gpio_set_function(sensor2::kPinSCL, GPIO_FUNC_I2C);
+ gpio_set_function(sensor2::kPinSDA, GPIO_FUNC_I2C);
+
+ StatusLED status_light1(sensor1::kPinStatusGreen, sensor1::kPinStatusRed);
+ StatusLED status_light2(sensor2::kPinStatusGreen, sensor2::kPinStatusRed);
+ BrightnessLED dist_light1(sensor1::kPinStatusPWM);
+ BrightnessLED dist_light2(sensor2::kPinStatusPWM);
+
+ BrightnessLED output_indicator(PICO_DEFAULT_LED_PIN);
+ SignalWriter output_writer(sensor1::kPinOutput);
+
+ printf("Initializing\n");
+
+ VL53L1X sensor1(1, status_light1, dist_light1, sensor1::kPinInterrupt,
+ sensor1::kPinShutdown);
+ VL53L1X sensor2(2, status_light2, dist_light2, sensor2::kPinInterrupt,
+ sensor2::kPinShutdown);
+
+ /* temporary*/ int n = 0;
+ double mean = 0;
+ double M2 = 0;
+
+ absolute_time_t last_reading = get_absolute_time();
+
+ // Measure and print continuously
+ while (1) {
+ sensor1.Start();
+ sensor1.WaitForData();
+ VL53L1X_Result_t result1 = sensor1.Read();
+ sensor1.Stop();
+
+ sensor2.Start();
+ sensor2.WaitForData();
+ VL53L1X_Result_t result2 = sensor2.Read();
+ sensor2.Stop();
+
+ double dist1 = static_cast<double>(result1.Distance) * 0.001;
+ double dist2 = static_cast<double>(result2.Distance) * 0.001;
+
+ // Estimates the center of the obstruction
+ // where 0 is at sensor 1 (left)
+ // and kSensorSeparation is at sensor 2 (right)
+ double width_of_obstruction = kSensorSeparation - (dist1 + dist2);
+
+ double left_estimate = (dist1 + (width_of_obstruction / 2));
+ double right_estimate =
+ (kSensorSeparation - dist2) - (width_of_obstruction / 2);
+
+ double averaged_estimate = (left_estimate + right_estimate) / 2;
+
+ bool data_good = sensor1.errors == 0 && result1.Status == 0 &&
+ sensor2.errors == 0 && result2.Status == 0 &&
+ width_of_obstruction > 0;
+
+ output_writer.SetEnabled(data_good);
+ output_writer.SetValue(averaged_estimate);
+ output_indicator.SetValue(data_good? averaged_estimate: 0);
+
+ /*Temporary */ if (data_good) {
+ n += 1;
+ double x = dist1 * 1000;
+ double delta = x - mean;
+ mean += delta / n;
+ M2 += delta * (x - mean);
+ printf("Std dev: %f mm\n", sqrt(M2 / (n - 1)));
+ } else {
+ n = 0;
+ mean = 0;
+ M2 = 0;
+ }
+
+ watchdog_update();
+ absolute_time_t now = get_absolute_time();
+ int64_t diff = absolute_time_diff_us(last_reading, now);
+ last_reading = now;
+ double period_ms = static_cast<double>(diff) * 0.001;
+
+ printf(
+ "Status = %2d, dist = %5d mm, Ambient = %3d, Signal = %5d, #ofSpads "
+ "= %3d\nStatus = %2d, "
+ "dist = %5d mm, "
+ "Ambient = %3d, Signal = %5d, "
+ "#ofSpads = %3d\nPeriod: %f Errors: %d %d %d %d\nx: %f, width: %f "
+ "data: %s\n",
+ result1.Status, result1.Distance, result1.Ambient, result1.SigPerSPAD,
+ result1.NumSPADs, result2.Status, result2.Distance, result2.Ambient,
+ result2.SigPerSPAD, result2.NumSPADs, period_ms, sensor1.errors,
+ sensor2.errors, sensor1.consecutive_errors, sensor2.consecutive_errors,
+ averaged_estimate, width_of_obstruction, data_good ? "good" : "bad");
+
+ // Try to reinitialize the sensor if it is in a bad state (eg. lost power
+ // but is now reconnected)
+ sensor1.MaybeAttemptRecovery();
+ sensor2.MaybeAttemptRecovery();
+
+ // allow the user to enter the bootloader without removing power or having
+ // to install a reset button
+ char user_input = getchar_timeout_us(0);
+ if (user_input == 'q') {
+ printf("Going down! entering bootloader\n");
+ reset_usb_boot(0, 0);
+ }
+
+ // Calibration mode
+ if (user_input == 'c') {
+ printf(
+ "Entering calibration mode\n"
+ "Please place 17%% gray target 100 mm from sensor 1 and press c "
+ "again to continue.\n");
+ while (true) {
+ user_input = getchar_timeout_us(0);
+ watchdog_update();
+ if (user_input == 'c') break;
+ }
+
+ printf("Taking 50 measurements\n");
+ int16_t offset = 0;
+ VL53L1X_CalibrateOffset(1, 100, &offset);
+ printf("Got an offset of %d\n", offset);
+
+ sleep_ms(1000);
+ }
+ }
+}
+
+} // namespace tof_controller
+} // namespace y2023
+
+int main() { y2023::tof_controller::main(); }