Swimmer Orientation with haptic feedback using ML

#include <Wire.h>
#include <Servo.h>
#include <MPU6050.h>

Servo leftMotor;
Servo rightMotor;

MPU6050 mpu;

float prevGyroAngle = 0.0;  // Previous gyroscope angle
float alpha = 0.98;         // Complementary filter constant

// Define gyro offset variables
float gyroXOffset = 0.0;
float gyroYOffset = 0.0;
float gyroZOffset = 0.0;

int calibrationButtonPin = 2;  // Pin to which the calibration button is connected
bool calibrateGyroFlag = false;

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

  leftMotor.attach(9);  // Connect left motor to pin 9
  rightMotor.attach(10); // Connect right motor to pin 10

  pinMode(calibrationButtonPin, INPUT_PULLUP);  // Set the button pin as input with pull-up resistor
  Wire.begin();
  mpu.initialize();

  if (!mpu.testConnection()) {
    Serial.println("MPU-6050 connection failed");
    while (1);
  }
}

void loop() {
  // Check if the calibration button is pressed
  if (digitalRead(calibrationButtonPin) == LOW) {
    calibrateGyroFlag = true;  // Set the flag to trigger gyroscope calibration
  }

  if (Serial.available() > 0) {
    // Read the orientation angle from the MaixPy Camera
    float cameraAngle = Serial.parseFloat();

    // Read gyroscope data
    float gyroAngle = readGyro();

    // Combine gyroscope and camera data using a complementary filter
    float combinedAngle = alpha * (prevGyroAngle + gyroAngle) + (1 - alpha) * cameraAngle;

    // Control the vibrational motors based on the combined angle
    controlMotors(combinedAngle);

    // Update the previous gyroscope angle for the next iteration
    prevGyroAngle = gyroAngle;
  }

  // Trigger gyroscope calibration if the flag is set
  if (calibrateGyroFlag) {
    calibrateGyro();
    calibrateGyroFlag = false;  // Reset the flag
  }
}

void calibrateGyro() {
  // Perform gyro calibration (adjust as needed)
  int numSamples = 1000;  // Adjust the number of samples as needed

  for (int i = 0; i < numSamples; i++) {
    int16_t gyroX, gyroY, gyroZ;
    mpu.getRotation(&gyroX, &gyroY, &gyroZ);

    // Accumulate gyro values for each axis
    gyroXOffset += gyroX;
    gyroYOffset += gyroY;
    gyroZOffset += gyroZ;

    delay(5);  // Adjust delay based on your specific requirements
  }

  // Calculate average offsets
  gyroXOffset /= numSamples;
  gyroYOffset /= numSamples;
  gyroZOffset /= numSamples;

  Serial.println("Gyroscope calibration completed!");
}

float readGyro() {
  // Read raw gyroscope data
  int16_t gyroX, gyroY, gyroZ;
  mpu.getRotation(&gyroX, &gyroY, &gyroZ);

  // Convert raw gyro values to degrees per second
  float gyroXrate = (gyroX - gyroXOffset) / 131.0;  // 131 LSB per degree/s
  float gyroYrate = (gyroY - gyroYOffset) / 131.0;
  float gyroZrate = (gyroZ - gyroZOffset) / 131.0;

  // Integrate gyro rates to get angle (simplified for demonstration purposes)
  float gyroAngleX = gyroXrate * 0.1;  // Integration with a simple time step
  float gyroAngleY = gyroYrate * 0.1;

  // Return the combined angle from the gyroscope (you may need to fine-tune this)
  return (gyroAngleX + gyroAngleY) / 2.0;
}

void controlMotors(float angle) {
  // Map the angle to motor speed (0 to 180)
  int motorSpeed = map(angle, -90, 90, 0, 180);

  // Adjust motor speeds based on the angle
  leftMotor.write(180 - motorSpeed);
  rightMotor.write(180 + motorSpeed);

  // Add a delay to avoid rapid changes (adjust as needed)
  delay(100);
}