Haptic Swim Assistant for Low Visibility Swimmers

#include <HardwareSerial.h>
#include <Wire.h>
#include <Notecard.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <SoftwareSerial.h>

#define PRODUCT_UID "<your project id here>"

Notecard notecard;
Adafruit_MPU6050 mpu;
unsigned char buffer_RTT[4] = { 0 };
uint8_t CS;
#define COM 0x55
HardwareSerial sensorLeft(2);  // Use UART2 for the first sensor
SoftwareSerial sensorRight(15, 4);

const int motorPinLeft = 18;
const int motorPinRight = 19;

int distanceLeft, distanceRight;

//Determined through collecting data with numerous tests -
const float thresholdXHigh = -0.35;  // Highest X
const float thresholdXLow = -2.25;   // Lowest X
const float thresholdYHigh = 0.3;    // Highest Y
const float thresholdYLow = -1.25;   // Lowest Y
const float thresholdZHigh = -8.4;   // Highest Z
const float thresholdZLow = -9.1;    // Lowest Z

const float thresholdRotXLow = -0.25;  // Example values, adjust as needed
const float thresholdRotXHigh = 0.25;
const float thresholdRotYLow = -0.25;
const float thresholdRotYHigh = 0.25;
const float thresholdRotZLow = -0.25;
const float thresholdRotZHigh = 0.25;

bool useFaceDownDetection = true;
bool faceDownHapticsRunning = false;
const unsigned long buzzDuration = 100;  // Duration of buzz in milliseconds
unsigned long nextBuzzLeft = 0;          //In millis
unsigned long nextBuzzRight = 0;
int intensityLeft = 0;
int intensityRight = 0;

// const unsigned long buzzDuration = 100;  // Duration of buzz in milliseconds
unsigned long buzzStartTimeLeft = 0;  // Tracks when buzzing starts for the left motor
unsigned long buzzStartTimeRight = 0;


void setup() {
  Serial.begin(115200);
  sensorLeft.begin(115200, SERIAL_8N1, 16, 17);  // Initialize Sensor1 on pins 16 (RX) and 17 (TX)
  sensorRight.begin(115200);

  Wire.begin();
  notecard.begin();

  pinMode(motorPinLeft, OUTPUT);
  pinMode(motorPinRight, OUTPUT);
  // mpu.initialize();
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    useFaceDownDetection = false;
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  Serial.println("Setup done");
}

void loop() {

  static unsigned long faceDownStartTime = 0;
  static bool faceDownAlertSent = false;

  distanceLeft = readSensor(sensorLeft, "left");
  if (distanceLeft > 0)
    updateHapticFeedback(distanceLeft, true);
  distanceRight = readSensor(sensorRight, "right");
  if (distanceRight > 0)
    updateHapticFeedback(distanceRight, false);
  provideHapticFeedback();

  if (useFaceDownDetection) {
    if (isFaceDown()) {
      if (faceDownStartTime == 0) {
        Serial.println("User is face down! Starting timer.");
        faceDownStartTime = millis();
        // Start different haptic feedback pattern for face down
      } else if ((millis() - faceDownStartTime > 60000) && !faceDownAlertSent) {
        faceDownHapticsRunning = true;

        analogWrite(motorPinLeft, 128);
        analogWrite(motorPinRight, 128);

        unsigned long timeElapsed = millis() - faceDownStartTime;
        unsigned long timeRemaining = 0;
        timeRemaining = 105000 - timeElapsed;
        Serial.print("User has been face down for ");
        Serial.print(timeElapsed);
        Serial.print(" ms. Time remaining before sending alert: ");
        Serial.println(timeRemaining);

        if (millis() - faceDownStartTime > 105000) {

          Serial.println("User is face down! Sending alert message!! ");
          sendMessageViaBlues("Haptic Swim Assistant Alert - USER IS FACE DOWN!!!!!");
          faceDownAlertSent = true;
        }
      } else if (!faceDownAlertSent) {
        Serial.print("User is currently face down ");
        Serial.print(millis() - faceDownStartTime);
        Serial.println(" milliseconds");
      }
    } else {
      if (faceDownHapticsRunning) {
        analogWrite(motorPinLeft, 0);  // Stop haptic feedback
        analogWrite(motorPinRight, 0);
      }
      faceDownStartTime = 0;
      faceDownAlertSent = false;
      faceDownHapticsRunning = false;
    }
  }
}

void sendMessageViaBlues(char *message) {

  J *req = notecard.newRequest("hub.set");
  if (req != NULL) {
    JAddStringToObject(req, "product", PRODUCT_UID);
    JAddStringToObject(req, "mode", "continuous");
    notecard.sendRequest(req);
  }

  req = notecard.newRequest("note.add");
  if (req != NULL) {
    JAddStringToObject(req, "file", "sensors.qo");
    JAddBoolToObject(req, "sync", true);
    J *body = JAddObjectToObject(req, "body");
    if (body) {
      JAddStringToObject(body, "message", message);
    }
    notecard.sendRequest(req);
  }
}

void provideHapticFeedback() {
  unsigned long currentTime = millis();

  // Handle left motor
  if (currentTime >= nextBuzzLeft && intensityLeft > 0 && buzzStartTimeLeft == 0) {
    Serial.println("Buzzing Left");
    analogWrite(motorPinLeft, intensityLeft);
    buzzStartTimeLeft = currentTime;  // Record start time of buzzing
    nextBuzzLeft = ULONG_MAX;         // Prevent immediate re-buzz
  } else if (buzzStartTimeLeft > 0 && (currentTime - buzzStartTimeLeft) >= buzzDuration) {
    Serial.println("Buzzing Left stopped");
    analogWrite(motorPinLeft, LOW);
    buzzStartTimeLeft = 0;  // Reset the start time
  }

  // Handle right motor
  if (currentTime >= nextBuzzRight && intensityRight > 0 && buzzStartTimeRight == 0) {
    analogWrite(motorPinRight, intensityRight);
    buzzStartTimeRight = currentTime;  // Record start time of buzzing
    nextBuzzRight = ULONG_MAX;         // Prevent immediate re-buzz
  } else if (buzzStartTimeRight > 0 && (currentTime - buzzStartTimeRight) >= buzzDuration) {
    analogWrite(motorPinRight, LOW);
    buzzStartTimeRight = 0;  // Reset the start time
  }
}

void updateHapticFeedback(long distance, bool isLeft) {
  unsigned long currentTime = millis();
  unsigned long nextBuzzTime;
  int intensity;
  if (distance > 600) {
    // No feedback if distance is more than 6 meters
    nextBuzzTime = ULONG_MAX;
    intensity = 0;
  } else if (distance <= 100) {
    // Full constant feedback at 1 meter or less
    nextBuzzTime = currentTime;
    intensity = 255;
  } else {
    // Gradually increase frequency and intensity as object gets closer
    long delayDuration = map(distance, 100, 600, 50, 500);
    nextBuzzTime = currentTime + delayDuration;
    intensity = map(distance, 100, 600, 255, 0);
  }

  if (isLeft) {
    if (nextBuzzLeft > nextBuzzTime || nextBuzzLeft <= 0) {
      nextBuzzLeft = nextBuzzTime;
    }
    intensityLeft = intensity;
  } else {
    if (nextBuzzRight > nextBuzzTime || nextBuzzRight <= 0) {
      nextBuzzRight = nextBuzzTime;
    }
    intensityRight = intensity;
  }
}

int readSensor(Stream &mySerial, String sensor) {
  mySerial.write(COM);
  delay(100);
  if (mySerial.available() > 0) {
    delay(4);
    if (mySerial.read() == 0xff) {
      buffer_RTT[0] = 0xff;
      for (int i = 1; i < 4; i++) {
        buffer_RTT[i] = mySerial.read();
      }
      CS = buffer_RTT[0] + buffer_RTT[1] + buffer_RTT[2];
      if (buffer_RTT[3] == CS) {
        int distance = (buffer_RTT[1] << 8) + buffer_RTT[2];
        return distance;
      }
    }
  }
  return 0;
}

bool isFaceDown() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Check if acceleration and rotation values are within thresholds
  bool xAccelInRange = (a.acceleration.x >= thresholdXLow) && (a.acceleration.x <= thresholdXHigh);
  bool yAccelInRange = (a.acceleration.y >= thresholdYLow) && (a.acceleration.y <= thresholdYHigh);
  bool zAccelInRange = (a.acceleration.z >= thresholdZLow) && (a.acceleration.z <= thresholdZHigh);

  bool xGyroInRange = (g.gyro.x >= thresholdRotXLow) && (g.gyro.x <= thresholdRotXHigh);
  bool yGyroInRange = (g.gyro.y >= thresholdRotYLow) && (g.gyro.y <= thresholdRotYHigh);
  bool zGyroInRange = (g.gyro.z >= thresholdRotZLow) && (g.gyro.z <= thresholdRotZHigh);

  // Combined check for face down
  bool isFaceDown = xAccelInRange && yAccelInRange && zAccelInRange && xGyroInRange && yGyroInRange && zGyroInRange;

  // Print only out-of-range values
  if (!xAccelInRange) Serial.println("Accel X out of range: " + String(a.acceleration.x));
  if (!yAccelInRange) Serial.println("Accel Y out of range: " + String(a.acceleration.y));
  if (!zAccelInRange) Serial.println("Accel Z out of range: " + String(a.acceleration.z));

  if (!xGyroInRange) Serial.println("Gyro X out of range: " + String(g.gyro.x));
  if (!yGyroInRange) Serial.println("Gyro Y out of range: " + String(g.gyro.y));
  if (!zGyroInRange) Serial.println("Gyro Z out of range: " + String(g.gyro.z));

  return isFaceDown;
}

//This one is just a simple test for your Blues Swan to get you up and running

#include <Wire.h>
#include <Notecard.h>

#define PRODUCT_UID "<put your project id here>"


Notecard notecard;

void setup() {
    Serial.begin(115200);
    while (!Serial);

    Wire.begin();
    notecard.begin();

    notecard.setDebugOutputStream(Serial);

    J *req = notecard.newRequest("hub.set");
    if (req != NULL) {
        JAddStringToObject(req, "product", PRODUCT_UID);
        JAddStringToObject(req, "mode", "continuous");
        notecard.sendRequest(req);
    }

    // Send a test message
    req = notecard.newRequest("note.add");
    if (req != NULL) {
        JAddStringToObject(req, "file", "sensors.qo");
        JAddBoolToObject(req, "sync", true);
        J *body = JAddObjectToObject(req, "body");
        if (body) {
            JAddStringToObject(body, "message", "Put in your test message here");
        }
        notecard.sendRequest(req);
    }
}

void loop() {
    // Do nothing here
}