Code for my pulse monitor

pulse.ino

#include <SparkFun_Bio_Sensor_Hub_Library.h>
#include <Wire.h>
#include <SPI.h>
#if defined(ESP32)
#include <WiFi.h>
#elif defined(ESP8266)
#include <ESP8266WiFi.h>
#else
#error "Only ESP32 and ESP8266 are supported!"
#endif
#include <WiFiUdp.h>
#include "pulse.h"


void setup(){
  Serial.begin(115200);
  delay(500);
  
  setup_mpu();
  setup_wifi();
  setup_bio();

  delay(500); 
}


void loop(){
  do_bpm();
  delay(10);
}


void send_udp(char line[64]) {
  Serial.println(line);
  // udp.beginPacket(influxHost, influxPort);
  // udp.write(line);
  // udp.endPacket();
}

void do_mpu(){
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,6);  // request a total of 14 registers
  int16_t ax,ay,az,gx,gy,gz;


  ax=Wire.read()<<8|Wire.read() / 16384;  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  ay=Wire.read()<<8|Wire.read() / 16384;  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  az=Wire.read()<<8|Wire.read() / 16384;  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x43); // Gyro data first register address 0x43
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,6); 
  gx=Wire.read()<<8|Wire.read() / 131;  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  gy=Wire.read()<<8|Wire.read() / 131;  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  gz=Wire.read()<<8|Wire.read() / 131;  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)


  char mpu_line[64];
  sprintf(mpu_line, "mpu ax=%di,ay=%di,az=%di,gx=%di,gy=%di,gz=%di",
    ax,
    ay,
    az,
    gx,
    gy,
    gz
  );

  send_udp(mpu_line);
}

void do_bpm() {
  uint16_t t = millis();
  static uint16_t t_old = t;
  uint16_t t_diff;

  uint8_t samples = bioHub.numSamplesOutFifo();       

  //read all samples in fifo and use most recent one
  while(samples){
    body = bioHub.readSensorBpm();
    samples--;
  }

  t_diff = t - t_old;
  
  if (t_diff >= 1000) {
    byte heartrate, confidence, oxygen, sts;
    char bpm_line[64];

    t_old += 1000;

    body = bioHub.readSensorBpm();
    heartrate = body.heartRate;
    confidence = body.confidence;
    oxygen = body.oxygen;
    sts = body.status;
    
    // Serial.printf("Heartrate: %d, Confidence: %d, Oxygen: %d, Status: %d\n", heartrate, confidence, oxygen, sts);
    sprintf(bpm_line, "bpm,confidence=%d,status=%d heartrate=%di,oxygen=%di", confidence, sts, heartrate, oxygen);
    send_udp(bpm_line);

    samples = bioHub.numSamplesOutFifo();
    do_mpu();
  }

  while(samples){
    body = bioHub.readSensorBpm();
    samples--;
  }
}

void setup_wifi(){
  WiFi.begin(ssid, pass);
  Serial.print("Connecting to wifi...");
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.print("  Connected! IP address: ");
  Serial.println(WiFi.localIP());
}


void setup_bio(){
  uint8_t result = bioHub.begin();
  if (result == 0) {
    Serial.println("Sensor started!");

    Serial.print("Configuring Sensor...  "); 
    uint8_t error = bioHub.configSensorBpm(MODE_ONE);
    
    if (error == 0) {
      Serial.println("Done!");

      error = bioHub.setPulseWidth(width);
      if (error == 0){
        Serial.printf("Pulse Width set to %d.\n", bioHub.readPulseWidth());
      } else {
        Serial.printf("Could not set Pulse Width! Error: %d\n", error);
      }

      error = bioHub.setSampleRate(samples);
      if (error == 0){
        Serial.printf("Sample Rate set to %d.\n", bioHub.readSampleRate());
      } else {
        Serial.printf("Could not set Sample Rate! Error: %d\n", error);
      }

    } else {
      Serial.printf("Error configuring sensor! Error: %d\n", error);
    }

  } else {
    Serial.printf("Could not start sensor! Error: %d\n", result);
  }
}


void setup_mpu(){
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission(true);
}