Runner tracker for treadmill

/*
*******************************************************************************
* Copyright (c) 2024 by JuanForero
* Equipped with M5StickC Plus and ToF hat
*
* Date: 2024/09/03
*******************************************************************************
  Please connect to hat,Use ToF Hat to detect distance and display distance
  data on the screen in real time with a corresponding tone for position
  feedback.

  Please install vl53l0x lib first(https://github.com/pololu/vl53l0x-arduino)
  lib in Sketch->Includ Library->Library Manager, search for vl53l0x
  Author:juanbiomech
*/

// Libraries
#include <VL53L0X.h>
#include <Wire.h>
#include <M5StickCPlus.h>

// Definitions
#define DISTANCE_FRONT   250
#define DISTANCE_BACK    750
#define DISTANCE_WINDOW   10
#define TONE_FRONT    3500
#define TONE_BACK      200
#define BAR_CENTER  120
#define BAR_WIDTH    20

// Variables
VL53L0X sensor;

uint16_t distance = 0;
uint16_t distance_n = 0;
uint16_t tone_frequency = TONE_BACK;

// Setup
void setup() {
  
  Wire.begin(0, 26, 100000UL);  //(0,26, 100000UL) for I2C of HAT connection

  M5.begin();

  M5.Lcd.setRotation(1);
  M5.Lcd.setTextSize(2);
  M5.Lcd.setTextColor(TFT_ORANGE);

  sensor.setTimeout(500);
  if (!sensor.init()) {
    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setCursor(80, 50);
    M5.Lcd.printf("%s", "Failed");
    
    Serial.println("Failed to detect and initialize sensor!");
    while (1) {
    }
  }

  sensor.startContinuous();

}

// Main
void loop() {

  //measure distance to target
  distance = sensor.readRangeContinuousMillimeters();
  distance_n = normDistance(distance);
  Serial.print(distance);
  if (sensor.timeoutOccurred()) {
      Serial.print(" TIMEOUT");
  }
  Serial.println();

  // display distance
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setCursor(10, 10);
  M5.Lcd.printf("Dist: %d (%d%%)", distance, distance_n);

  // display position bar
  positionBar(distance_n);

  // generate tone based on distance
  toneGenerator(distance_n);

}

// Distance normalization
uint16_t normDistance(uint16_t dist) {
  if (dist<DISTANCE_FRONT)
    dist = DISTANCE_FRONT;
  if (dist>DISTANCE_BACK)
    dist = DISTANCE_BACK;
  return map(dist, DISTANCE_FRONT, DISTANCE_BACK, 0, 100);
}

// Sound functions
void toneGenerator(uint16_t dist_norm) {
  uint16_t freq = map(dist_norm, 0, 100, TONE_FRONT, TONE_BACK);
  
  M5.Lcd.setCursor(10, 115);
  M5.Lcd.printf("Tone: %d Hz", freq);
  M5.Beep.tone(freq);
  M5.Beep.update();
  delay(100);
}

// Position bar functions
void positionBar(uint16_t pos) {
  int x = map(pos, 0, 100, 230, 10);
  M5.Lcd.fillRoundRect(10, 50, 230, 50, 10, DARKGREY);
  if (pos<50) {
    M5.Lcd.fillRoundRect(BAR_CENTER, 50, x-BAR_CENTER, 50, 10, RED);
  } else {
    M5.Lcd.fillRoundRect(x, 50, BAR_CENTER-x, 50, 10, BLUE);
  }
  M5.Lcd.fillRoundRect(BAR_CENTER-BAR_WIDTH/2, 45, BAR_WIDTH, 60, 10, GREEN);
}