Team Coconut's Maraca Controller

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

SoftwareSerial mySerial(3, 2); // TX, RX to HC-05
MPU6050 mpu;

// LED pins
const int ledForward = 7;
const int ledBackward = 8;
const int ledLeft = 9;
const int ledRight = 10;
const int ledStop = 11;

// Shake threshold
const float shakeThreshold = 0.5;

// Calibration offsets
float offsetX = 0;
float offsetY = 0;
float offsetZ = 0;

void setup() {
  Serial.begin(9600);
  mySerial.begin(38400);

  Wire.begin();
  Wire.setClock(100000); // Ensure safe 100kHz I2C
  delay(100);            // Allow MPU to power up

  mpu.initialize();
  mpu.setSleepEnabled(false); // Wake up MPU
  delay(100);

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

  // LEDs
  pinMode(ledForward, OUTPUT);
  pinMode(ledBackward, OUTPUT);
  pinMode(ledLeft, OUTPUT);
  pinMode(ledRight, OUTPUT);
  pinMode(ledStop, OUTPUT);

  // Set LEDs off at start
  digitalWrite(ledForward, LOW);
  digitalWrite(ledBackward, LOW);
  digitalWrite(ledLeft, LOW);
  digitalWrite(ledRight, LOW);
  digitalWrite(ledStop, LOW);

  Serial.println("Calibrating MPU... keep still.");
  calibrateMPU();
  Serial.println("Calibration complete!");

  Serial.println("Maraca Motion Controller Ready!");
}

void loop() {
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

  // Apply calibration offsets
  float x = (ax / 16384.0) - offsetX;
  float y = (ay / 16384.0) - offsetY;
  float z = (az / 16384.0) - offsetZ;

  String direction = detectShakeDirection(x, y, z);

  if (direction != "") {
    indicateDirection(direction);    
    sendBluetoothCommand(direction); 
    delay(300); // small cooldown
  }

  delay(50);
}

// ----------- DETECTION ------------
String detectShakeDirection(float x, float y, float z) {
  if (x > shakeThreshold) return "Right";
  if (x < -shakeThreshold) return "Left";
  if (y > shakeThreshold) return "Up";
  if (y < -shakeThreshold) return "Down";
  return "";
}

// ----------- SEND COMMANDS --------
void sendBluetoothCommand(String direction) {
  if (direction == "Down") {
    mySerial.println("F");
    Serial.println("F");
  }
  else if (direction == "Up") {
    mySerial.println("B");
    Serial.println("B");
  }
  else if (direction == "Left") {
    mySerial.println("L");
    Serial.println("L");
  }
  else if (direction == "Right") {
    mySerial.println("R");
    Serial.println("R");
  }
}

// ----------- LED FEEDBACK ---------
void indicateDirection(String dir) {
  digitalWrite(ledForward,  dir == "Down");
  digitalWrite(ledBackward, dir == "Up");
  digitalWrite(ledLeft,     dir == "Left");
  digitalWrite(ledRight,    dir == "Right");

  delay(300);

  digitalWrite(ledForward, LOW);
  digitalWrite(ledBackward, LOW);
  digitalWrite(ledLeft, LOW);
  digitalWrite(ledRight, LOW);
}

// ----------- CALIBRATION -----------
void calibrateMPU() {
  long sumX = 0, sumY = 0, sumZ = 0;

  for (int i = 0; i < 200; i++) {
    int16_t ax, ay, az;
    mpu.getAcceleration(&ax, &ay, &az);

    sumX += ax;
    sumY += ay;
    sumZ += az;

    delay(10);
  }

  offsetX = (sumX / 200.0) / 16384.0;
  offsetY = (sumY / 200.0) / 16384.0;
  offsetZ = (sumZ / 200.0) / 16384.0;

  Serial.print("Offsets: ");
  Serial.print(offsetX); Serial.print(", ");
  Serial.print(offsetY); Serial.print(", ");
  Serial.println(offsetZ);
}