Smart Badminton Racket Tracker

#include <Arduino.h>
#include <Wire.h>
#include <ArduinoJson.h>
#include <Adafruit_ICM20948.h>
#include <Adafruit_Sensor.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>


// Forward declarations
void IRAM_ATTR onVibration();
float magnitude3(float x, float y, float z);
void setupBle();

constexpr int SDA_PIN = 21;
constexpr int SCL_PIN = 22;
constexpr int IMU_INT_PIN = 27;
constexpr int IMPACT_PIN = 26;
constexpr int GRIP_PIN = 34;
constexpr uint16_t SAMPLE_HZ = 100;
constexpr uint32_t SAMPLE_PERIOD_US = 1000000UL / SAMPLE_HZ;
constexpr float IMPACT_ACCEL_THRESHOLD_G = 5.0f;
constexpr float RAD_TO_DEG = 57.2957795f;

const char *DEVICE_NAME = "SmartRacket-001";
const char *SERVICE_UUID = "7d315ef1-8e28-4c6c-a46b-e719b8e21a01";
const char *STREAM_UUID = "7d315ef2-8e28-4c6c-a46b-e719b8e21a01";

Adafruit_ICM20948 icm;
BLECharacteristic *streamCharacteristic = nullptr;
volatile bool vibrationEdge = false;
bool clientConnected = false;
uint32_t lastSampleUs = 0;
uint32_t sequenceNumber = 0;
uint32_t swingCount = 0;
bool inSwing = false;

void IRAM_ATTR onVibration() {
  vibrationEdge = true;
}

class ServerCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer *server) override {
    clientConnected = true;
  }

  void onDisconnect(BLEServer *server) override {
    clientConnected = false;
    BLEDevice::startAdvertising();
  }
};

float magnitude3(float x, float y, float z) {
  return sqrtf(x * x + y * y + z * z);
}

void setupBle() {
  BLEDevice::init(DEVICE_NAME);
  BLEServer *server = BLEDevice::createServer();
  server->setCallbacks(new ServerCallbacks());
  BLEService *service = server->createService(SERVICE_UUID);
  streamCharacteristic = service->createCharacteristic(
      STREAM_UUID,
      BLECharacteristic::PROPERTY_NOTIFY | BLECharacteristic::PROPERTY_READ);
  streamCharacteristic->addDescriptor(new BLE2902());
  streamCharacteristic->setValue("{\"status\":\"ready\"}");
  service->start();
  BLEAdvertising *advertising = BLEDevice::getAdvertising();
  advertising->addServiceUUID(SERVICE_UUID);
  advertising->setScanResponse(true);
  BLEDevice::startAdvertising();
}

void setup() {
  Serial.begin(115200);
  pinMode(IMPACT_PIN, INPUT);
  pinMode(IMU_INT_PIN, INPUT);
  analogReadResolution(12);
  attachInterrupt(digitalPinToInterrupt(IMPACT_PIN), onVibration, RISING);

  Wire.begin(SDA_PIN, SCL_PIN);
  if (!icm.begin_I2C(0x69, &Wire)) {
    Serial.println("ICM-20948 tidak ditemukan pada alamat I2C 0x69.");
    while (true) {
      delay(1000);
    }
  }

  icm.setAccelRange(ICM20948_ACCEL_RANGE_16_G);
  icm.setGyroRange(ICM20948_GYRO_RANGE_2000_DPS);
  icm.setAccelRateDiv(10);
  icm.setGyroRateDiv(10);
  setupBle();
  lastSampleUs = micros();
}

void loop() {
  const uint32_t nowUs = micros();
  if (static_cast<uint32_t>(nowUs - lastSampleUs) < SAMPLE_PERIOD_US) {
    return;
  }
  lastSampleUs += SAMPLE_PERIOD_US;

  sensors_event_t accel;
  sensors_event_t gyro;
  sensors_event_t temperature;
  sensors_event_t mag;
  icm.getEvent(&accel, &gyro, &temperature, &mag);

  const float ax = accel.acceleration.x;
  const float ay = accel.acceleration.y;
  const float az = accel.acceleration.z;
  const float gxDps = gyro.gyro.x * RAD_TO_DEG;
  const float gyDps = gyro.gyro.y * RAD_TO_DEG;
  const float gzDps = gyro.gyro.z * RAD_TO_DEG;
  const float accelG = magnitude3(ax, ay, az) / 9.80665f;
  const float gyroDps = magnitude3(gxDps, gyDps, gzDps);
  const int gripRaw = analogRead(GRIP_PIN);

  // Orientation is a tilt/heading estimate for streaming. Gateway/server should
  // perform calibrated AHRS fusion for analysis and replay.
  const float rollDeg = atan2f(ay, az) * RAD_TO_DEG;
  const float pitchDeg = atan2f(-ax, sqrtf(ay * ay + az * az)) * RAD_TO_DEG;
  float headingDeg = atan2f(mag.magnetic.y, mag.magnetic.x) * RAD_TO_DEG;
  if (headingDeg < 0.0f) {
    headingDeg += 360.0f;
  }

  const bool impactByImu = accelG >= IMPACT_ACCEL_THRESHOLD_G;
  const bool impact = vibrationEdge || impactByImu;
  vibrationEdge = false;

  const bool swingingNow = gyroDps > 180.0f || accelG > 1.8f;
  if (swingingNow && !inSwing) {
    ++swingCount;
  }
  inSwing = swingingNow;

  StaticJsonDocument<512> packet;
  packet["schema"] = "smart-racket.v1";
  packet["player_id"] = "001";
  packet["device_id"] = DEVICE_NAME;
  packet["sequence"] = sequenceNumber++;
  packet["timestamp_ms"] = millis();
  packet["sample_hz"] = SAMPLE_HZ;
  JsonArray acceleration = packet.createNestedArray("acceleration_mps2");
  acceleration.add(ax);
  acceleration.add(ay);
  acceleration.add(az);
  JsonArray gyroscope = packet.createNestedArray("gyroscope_dps");
  gyroscope.add(gxDps);
  gyroscope.add(gyDps);
  gyroscope.add(gzDps);
  JsonArray magnetic = packet.createNestedArray("magnetic_ut");
  magnetic.add(mag.magnetic.x);
  magnetic.add(mag.magnetic.y);
  magnetic.add(mag.magnetic.z);
  JsonObject orientation = packet.createNestedObject("orientation_deg");
  orientation["roll"] = rollDeg;
  orientation["pitch"] = pitchDeg;
  orientation["heading"] = headingDeg;
  packet["acceleration_magnitude_g"] = accelG;
  packet["angular_speed_dps"] = gyroDps;
  packet["grip_raw_12bit"] = gripRaw;
  packet["impact"] = impact;
  packet["swing_active"] = swingingNow;
  packet["swing_count"] = swingCount;

  char json[512];
  const size_t length = serializeJson(packet, json, sizeof(json));
  Serial.write(reinterpret_cast<const uint8_t *>(json), length);
  Serial.println();
  if (clientConnected) {
    streamCharacteristic->setValue(reinterpret_cast<uint8_t *>(json), length);
    streamCharacteristic->notify();
  }
}