#include <Wire.h>

// =====================================================
// ROBO RUMBLE - MPU6050 GYROSCOPE TEST
//
// STM32F103C8T6 Blue Pill
//
// MPU6050:
// SDA -> PB7
// SCL -> PB6
//
// PURPOSE:
// 1. Verify MPU6050 identity
// 2. Wake MPU6050
// 3. Read X, Y and Z gyro values
// 4. Convert raw gyro values to degrees/second
// =====================================================


// =====================================================
// MPU6050 SETTINGS
// =====================================================

#define MPU_ADDR       0x68

#define PWR_MGMT_1     0x6B
#define GYRO_CONFIG    0x1B
#define GYRO_XOUT_H    0x43
#define WHO_AM_I       0x75


// =====================================================
// WRITE MPU6050 REGISTER
// =====================================================

bool writeMPURegister(uint8_t reg, uint8_t value)
{
  Wire.beginTransmission(MPU_ADDR);

  Wire.write(reg);
  Wire.write(value);

  uint8_t result = Wire.endTransmission();

  return (result == 0);
}


// =====================================================
// READ MPU6050 REGISTER
// =====================================================

bool readMPURegister(uint8_t reg, uint8_t &value)
{
  Wire.beginTransmission(MPU_ADDR);

  Wire.write(reg);

  uint8_t result =
    Wire.endTransmission(false);

  if (result != 0)
  {
    return false;
  }

  uint8_t received =
    Wire.requestFrom(
      (uint8_t)MPU_ADDR,
      (uint8_t)1
    );

  if (received != 1)
  {
    return false;
  }

  value = Wire.read();

  return true;
}


// =====================================================
// READ GYROSCOPE
// =====================================================

bool readGyroscope(
  int16_t &gyroX,
  int16_t &gyroY,
  int16_t &gyroZ
)
{
  Wire.beginTransmission(MPU_ADDR);

  Wire.write(GYRO_XOUT_H);

  uint8_t result =
    Wire.endTransmission(false);

  if (result != 0)
  {
    return false;
  }


  uint8_t received =
    Wire.requestFrom(
      (uint8_t)MPU_ADDR,
      (uint8_t)6
    );


  if (received != 6)
  {
    return false;
  }


  gyroX =
    ((int16_t)Wire.read() << 8) |
    Wire.read();


  gyroY =
    ((int16_t)Wire.read() << 8) |
    Wire.read();


  gyroZ =
    ((int16_t)Wire.read() << 8) |
    Wire.read();


  return true;
}


// =====================================================
// SETUP
// =====================================================

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

  delay(500);


  // ---------------------------------------------------
  // Start I2C
  // ---------------------------------------------------

  Wire.setSDA(PB7);
  Wire.setSCL(PB6);

  Wire.begin();

  delay(500);


  Serial1.println();
  Serial1.println("========================================");
  Serial1.println(" ROBO RUMBLE - MPU6050 GYRO TEST");
  Serial1.println("========================================");

  Serial1.println();

  Serial1.println("[INFO] SDA = PB7");
  Serial1.println("[INFO] SCL = PB6");
  Serial1.println("[INFO] MPU6050 address = 0x68");

  Serial1.println();


  // ===================================================
  // WHO_AM_I TEST
  // ===================================================

  uint8_t identity = 0;

  if (readMPURegister(WHO_AM_I, identity))
  {
    Serial1.print("WHO_AM_I = 0x");

    if (identity < 0x10)
    {
      Serial1.print("0");
    }

    Serial1.println(identity, HEX);


    if (identity == 0x68)
    {
      Serial1.println(
        "[PASS] MPU6050 identity confirmed"
      );
    }

    else
    {
      Serial1.println(
        "[WARNING] Unexpected device identity"
      );
    }
  }

  else
  {
    Serial1.println(
      "[FAIL] Could not read WHO_AM_I"
    );
  }


  // ===================================================
  // WAKE MPU6050
  // ===================================================

  if (
    writeMPURegister(
      PWR_MGMT_1,
      0x00
    )
  )
  {
    Serial1.println(
      "[PASS] MPU6050 awakened"
    );
  }

  else
  {
    Serial1.println(
      "[FAIL] Could not wake MPU6050"
    );
  }


  // ===================================================
  // GYRO RANGE
  //
  // 0x00 = +/-250 degrees/sec
  //
  // Sensitivity = 131 LSB/(deg/s)
  // ===================================================

  if (
    writeMPURegister(
      GYRO_CONFIG,
      0x00
    )
  )
  {
    Serial1.println(
      "[PASS] Gyroscope configured to +/-250 deg/s"
    );
  }


  Serial1.println();
  Serial1.println(
    "Starting gyroscope measurements..."
  );
}


// =====================================================
// MAIN LOOP
// =====================================================

void loop()
{
  int16_t rawX = 0;
  int16_t rawY = 0;
  int16_t rawZ = 0;


  if (
    readGyroscope(
      rawX,
      rawY,
      rawZ
    )
  )
  {
    // -------------------------------------------------
    // Convert raw values to degrees/second
    //
    // +/-250 deg/s setting:
    //
    // 131 LSB = 1 degree/second
    // -------------------------------------------------

    float gyroX =
      rawX / 131.0f;

    float gyroY =
      rawY / 131.0f;

    float gyroZ =
      rawZ / 131.0f;


    Serial1.println();
    Serial1.println(
      "----------------------------------------"
    );


    Serial1.print("GYRO X : ");
    Serial1.print(gyroX, 2);
    Serial1.println(" deg/s");


    Serial1.print("GYRO Y : ");
    Serial1.print(gyroY, 2);
    Serial1.println(" deg/s");


    Serial1.print("GYRO Z : ");
    Serial1.print(gyroZ, 2);
    Serial1.println(" deg/s");


    // -------------------------------------------------
    // Basic yaw interpretation
    // -------------------------------------------------

    Serial1.print("YAW STATUS : ");


    if (gyroZ > 5.0)
    {
      Serial1.println(
        "ROTATING"
      );
    }

    else if (gyroZ < -5.0)
    {
      Serial1.println(
        "ROTATING OPPOSITE DIRECTION"
      );
    }

    else
    {
      Serial1.println(
        "STRAIGHT / STATIONARY"
      );
    }
  }

  else
  {
    Serial1.println(
      "[ERROR] Failed to read gyroscope"
    );
  }


  delay(500);
}