#include <Wire.h>
#include <MPU6050_tockn.h>
#include <NewPing.h>

MPU6050 mpu6050(Wire);
NewPing sonar(2, 3, 200);   // Trigger pin (D2), Echo pin (D3), Max distance (200 cm)

const int vibrationMotorPin = 9;  // Pin to which the vibration motor is connected
int count =0;
void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu6050.begin();
  
  pinMode(vibrationMotorPin, OUTPUT);
}

void loop() {
  float distance = getDistance();
  float tiltAngle = getTiltAngle();
  
  // Adjust the threshold based on your environment and preferences
  float obstacleThreshold = 30.0;  // Threshold for obstacle detection in centimeters
  float tiltThreshold = 30.0;      // Threshold for tilt angle in degrees

  if (distance < obstacleThreshold) {
    // Obstacle detected, activate vibration
    Serial.println("STOP");
    Serial.println(count);
    count +=1;
    activateVibration();
  } else if (tiltAngle > tiltThreshold) {
    // Serial.println("STOP");
    // Tilt detected, possible change in direction, activate vibration
    activateVibration();
  } else {
    // No obstacle or significant tilt, deactivate vibration
    deactivateVibration();
  }
}

float getDistance() {
  delay(50);  // Wait for previous ping to finish
  return sonar.ping_cm();
}

float getTiltAngle() {
  mpu6050.update();
  return mpu6050.getAngleY();
}

void activateVibration() {
  digitalWrite(vibrationMotorPin, HIGH);
}

void deactivateVibration() {
  digitalWrite(vibrationMotorPin, LOW);
}