Self Balancing Robot

#include <PID_v2.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define motorA_IN1 6
#define motorA_IN2 7
#define motorA_ENA 11
#define motorB_IN3 4
#define motorB_IN4 5
#define motorB_ENB 10
MPU6050 mpu(0x68);
double setpoint = 0;
double input;
double output;
double kp = 40, ki = 0.8, kd = 1.2;
PID balancePID(&input, &output, &setpoint, kp, ki, kd, DIRECT);
unsigned long lastTime = 0;
const int sampleTime = 10;
void setup() {
  Serial.begin(115200);
  Serial.println("Self-Balancing Robot Starting...");
  Wire.begin();
  // Untuk LGT8F328 @32MHz, nilai TWBR 32 sangat penting agar I2C stabil
  TWBR = 32;
  delay(1000);
  Serial.println("Initializing Sensor...");
  // Membangunkan sensor secara manual (Wake up)
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission();
  delay(100);
  // Deklarasi variabel chipID di sini agar tidak error 'not declared'
  byte chipID = 0;
  // Membaca ID perangkat secara langsung dari register 0x75 (WHO_AM_I)
  Wire.beginTransmission(0x68);
  Wire.write(0x75);
  Wire.endTransmission();
  Wire.requestFrom(0x68, 1);
  if (Wire.available()) {
    chipID = Wire.read();
    Serial.print("Device ID found: 0x");
    Serial.println(chipID, HEX);
  }
  mpu.initialize();
  // Modifikasi pengecekan koneksi: Izinkan ID 0x68 (MPU) atau 0x70 (QMI)
  if (chipID == 0x68 || chipID == 0x70) {
    Serial.println("Sensor connection successful");
  } else {
    Serial.println("Warning: Sensor connection failed or unknown ID");
    // Kita tetap lanjut (tidak pakai while(1)) untuk mengetes apakah data terbaca
    Serial.println("Attempting to proceed anyway...");
  }
  calibrateSensor();
  pinMode(motorA_IN1, OUTPUT);
  pinMode(motorA_IN2, OUTPUT);
  pinMode(motorA_ENA, OUTPUT);
  pinMode(motorB_IN3, OUTPUT);
  pinMode(motorB_IN4, OUTPUT);
  pinMode(motorB_ENB, OUTPUT);
  balancePID.SetMode(AUTOMATIC);
  balancePID.SetSampleTime(sampleTime);
  balancePID.SetOutputLimits(-255, 255);
  Serial.println("Initialization complete!");
}
double getBalanceAngle() {
  static double gyroAngle = 0;
  static unsigned long prevTime = 0;
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  double accelAngle = atan2(ax, az) * 180 / PI;
  unsigned long currentTime = millis();
  double dt = (currentTime - prevTime) / 1000.0;
  prevTime = currentTime;
  double gyroRate = gy / 131.0;
  gyroAngle += gyroRate * dt;
  double filteredAngle = 0.95 * gyroAngle + 0.05 * accelAngle;
  gyroAngle = filteredAngle;  // Update gyro angle
  return filteredAngle;
}
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
  leftSpeed = constrain(leftSpeed, -255, 255);
  rightSpeed = constrain(rightSpeed, -255, 255);
  if (leftSpeed >= 0) {
    digitalWrite(motorA_IN1, HIGH);
    digitalWrite(motorA_IN2, LOW);
    analogWrite(motorA_ENA, leftSpeed);
  } else {
    digitalWrite(motorA_IN1, LOW);
    digitalWrite(motorA_IN2, HIGH);
    analogWrite(motorA_ENA, abs(leftSpeed));
  }
  if (rightSpeed >= 0) {
    digitalWrite(motorB_IN3, HIGH);
    digitalWrite(motorB_IN4, LOW);
    analogWrite(motorB_ENB, rightSpeed);
  } else {
    digitalWrite(motorB_IN3, LOW);
    digitalWrite(motorB_IN4, HIGH);
    analogWrite(motorB_ENB, abs(rightSpeed));
  }
}
void stopMotors() {
  analogWrite(motorA_ENA, 0);
  analogWrite(motorB_ENB, 0);
}
void loop() {
  unsigned long currentTime = millis();
  if (currentTime - lastTime >= sampleTime) {
    lastTime = currentTime;
    input = getBalanceAngle();
    if (abs(input) > 45) {
      stopMotors();
      Serial.println("Robot fallen - stopping motors");
      return;
    }
    balancePID.Compute();
    int motorSpeed = (int)output;
    setMotorSpeeds(motorSpeed, motorSpeed);
    Serial.print("Angle: ");
    Serial.print(input);
    Serial.print(" | Output: ");
    Serial.println(output);
  }
}
void calibrateSensor() {
  Serial.println("Calibrating sensor - hold robot vertical...");
  long accelSum = 0, gyroSum = 0;
  int samples = 1000;
  for (int i = 0; i < samples; i++) {
    int16_t ax, ay, az, gx, gy, gz;
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    accelSum += ax;
    gyroSum += gy;
    delay(3);
  }
  int accelOffset = accelSum / samples;
  int gyroOffset = gyroSum / samples;
  Serial.print("Calibration complete. Offsets - Accel: ");
  Serial.print(accelOffset);
  Serial.print(", Gyro: ");
  Serial.println(gyroOffset);
}