//  ESP32 SELF-BALANCING ROBOT with Web-Tuning
//  WiFi: The ESP32 runs as Access Point
//  SSID: BalanceBot   Password: 12345678
//  Open browser http://192.168.4.1
//  NVS note: all tunable values from the web are saved to flash on "Save".

// Initialize the Library used
#include <Wire.h>
#include <WiFi.h>
#include <WebServer.h>
#include <Preferences.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <FluxGarage_RoboEyes.h>
#include <math.h>

// Initialize the 0.96 Oled Display
#define SCREEN_WIDTH  128
#define SCREEN_HEIGHT  64
#define OLED_RESET     -1
#define OLED_ADDR_L   0x3C
#define OLED_ADDR_R   0x3D

Adafruit_SSD1306 displayL(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire,  OLED_RESET);
Adafruit_SSD1306 displayR(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire,  OLED_RESET);

// Initialize the RoboEyes Library
RoboEyes<Adafruit_SSD1306> roboEyesL(displayL);
RoboEyes<Adafruit_SSD1306> roboEyesR(displayR);

// Initialize Separate I2C Connection
TwoWire I2CMPU = TwoWire(1);
Adafruit_MPU6050 mpu;

// Initialize L298N Motor Driver Pins
const int ENA = 32, IN1 = 25, IN2 = 33;
const int ENB = 14, IN3 = 27, IN4 = 26;
const int PWM_FREQ = 1000, PWM_RES = 8;

// Initialize the Default values of PID loop and Oled Display
// These values are used on first boot when NVS is blank
#define KP_DEF        22.0f
#define KI_DEF         0.3f
#define KD_DEF         1.4f
#define SP_DEF         0.0f
#define PM_DEF         1.0f
#define MPWM_DEF         80
#define TG_DEF         1.0f
#define FALL_DEF      35.0f
#define WORRIED_DEF   20.0f

// Sentinel value stored in NVS when no boot angle has been measured yet.
// 999.0 is physically impossible for a tilt angle, so it safely means "not set".
#define BOOT_ANGLE_UNSET  999.0f

// Initialize the variables of the PID loop and Oled Display
float Kp              = KP_DEF;
float Ki              = KI_DEF;
float Kd              = KD_DEF;
float setpoint        = SP_DEF;
float powerMultiplier = PM_DEF;
int   minPWM          = MPWM_DEF;
float tiltGain        = TG_DEF;
float fallAngle       = FALL_DEF;
float worriedAngle    = WORRIED_DEF;

// Initialize the PID State
float angleDeg        = 0.0f;
float gyroRateDeg     = 0.0f;
float integral        = 0.0f;
float lastOutput      = 0.0f;
float prevError       = 0.0f;
float dynamicSetpoint = 0.0f;
unsigned long lastMicrosTs = 0;

// Initialize the face state
bool faceIsX = false;

// Initialize Web-Server name and password
const char* ssid     = "BalanceBot";
const char* password = "12345678";
WebServer server(80);

// Initialize the NVS (Non-volatile storage)
Preferences prefs;
#define NVS_NS  "pidcfg"

// NVS and PID Parameters
void loadPrefs() {
  prefs.begin(NVS_NS, true);
  Kp              = prefs.getFloat("kp",   KP_DEF);
  Ki              = prefs.getFloat("ki",   KI_DEF);
  Kd              = prefs.getFloat("kd",   KD_DEF);
  setpoint        = prefs.getFloat("sp",   SP_DEF);
  powerMultiplier = prefs.getFloat("pm",   PM_DEF);
  minPWM          = prefs.getInt("mpwm",   MPWM_DEF);
  tiltGain        = prefs.getFloat("tg",   TG_DEF);
  fallAngle       = prefs.getFloat("fall", FALL_DEF);
  worriedAngle    = prefs.getFloat("worr", WORRIED_DEF);
  prefs.end();
}

void savePrefs() {
  prefs.begin(NVS_NS, false);
  prefs.putFloat("kp",   Kp);
  prefs.putFloat("ki",   Ki);
  prefs.putFloat("kd",   Kd);
  prefs.putFloat("sp",   setpoint);
  prefs.putFloat("pm",   powerMultiplier);
  prefs.putInt("mpwm",   minPWM);
  prefs.putFloat("tg",   tiltGain);
  prefs.putFloat("fall", fallAngle);
  prefs.putFloat("worr", worriedAngle);
  prefs.end();
}

void resetPrefs() {
  Kp = KP_DEF; Ki = KI_DEF; Kd = KD_DEF;
  setpoint = SP_DEF; powerMultiplier = PM_DEF;
  minPWM = MPWM_DEF; tiltGain = TG_DEF;
  fallAngle = FALL_DEF; worriedAngle = WORRIED_DEF;
  integral = 0.0f; prevError = 0.0f;
  savePrefs();
}

// Boot Angle sequence or operation
float loadBootAngle() {
  prefs.begin(NVS_NS, true);
  float val = prefs.getFloat("bangle", BOOT_ANGLE_UNSET);
  prefs.end();
  return val;
}

// Saves the measured boot angle to NVS.
void saveBootAngle(float angle) {
  prefs.begin(NVS_NS, false);
  prefs.putFloat("bangle", angle);
  prefs.end();
}

// Reset the found boot angle value
void clearBootAngle() {
  prefs.begin(NVS_NS, false);
  prefs.remove("bangle");
  prefs.end();
}

// HTML Web-Server Wifi-Access Section
const char* htmlPage = R"rawliteral(
<!DOCTYPE html><html><head>
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width,initial-scale=1">
<title>PID Tuner</title>
<style>
  body{font-family:sans-serif;background:#111;color:#eee;max-width:480px;margin:30px auto;padding:0 14px}
  h2{color:#4fc3f7;text-align:center;margin-bottom:20px}
  .card{background:#1e1e1e;border-radius:10px;padding:14px;margin-bottom:14px;border:1px solid #333}
  label{font-size:0.8rem;color:#90a4ae}
  input{width:100%;background:#263238;border:1px solid #455a64;color:#eee;
        border-radius:6px;padding:7px 9px;font-size:0.95rem;box-sizing:border-box;margin-top:3px}
  .row{display:flex;gap:10px;margin-bottom:10px}.field{flex:1}
  .hint{font-size:0.68rem;color:#546e7a;margin-top:2px}
  button{width:100%;padding:11px;border:none;border-radius:8px;font-size:1rem;font-weight:700;cursor:pointer;margin-top:5px}
  .save{background:#fbc02d;color:#111}
  .reset{background:#37474f;color:#cfd8dc;width:50%;display:block;margin:10px auto 0 auto;padding:8px;font-size:0.85rem}
  .bootbtn{background:#37474f;color:#cfd8dc;width:50%;display:block;margin:6px auto 0 auto;padding:8px;font-size:0.85rem}
  .trim-btn{background:#455a64;color:white;width:40px;padding:7px;margin-top:3px;border-radius:6px;font-size:1.2rem;font-weight:bold;line-height:1}
  #tel{background:#263238;border-radius:8px;padding:10px;font-size:0.8rem;line-height:1.8;margin-bottom:10px;text-align:center}
  .saved{background:#1b5e20;color:#a5d6a7;padding:8px 12px;border-radius:7px;margin-bottom:10px;display:none}
  .bootmsg{background:#1a3a4a;color:#80cbc4;padding:8px 12px;border-radius:7px;margin-bottom:10px;display:none;font-size:0.85rem}
</style></head><body>
<h2>BalanceBot PID Tuner</h2>
<div class="card"><b style="font-size:0.85rem;color:#80cbc4;display:block;text-align:center;margin-bottom:5px">Live Telemetry</b>
<div id="tel">Waiting...</div></div>
<div id="savedMsg" class="saved">Saved to flash memory.</div>
<div id="bootMsg" class="bootmsg">Boot Angle cleared. Reboot the robot to remeasure.</div>
<div class="card">
<form onsubmit="sendVals();return false;">
  <div class="row">
    <div class="field"><label>Kp</label><input id="kp" type="number" step="any" value="22.0"><div class="hint">Proportional gain</div></div>
    <div class="field"><label>Ki</label><input id="ki" type="number" step="any" value="0.3"><div class="hint">Integral gain</div></div>
  </div>
  <div class="row">
    <div class="field"><label>Kd</label><input id="kd" type="number" step="any" value="1.4"><div class="hint">Derivative gain</div></div>
    <div class="field"><label>Setpoint Correction &deg;</label>
      <div style="display:flex;gap:5px">
        <button class="trim-btn" type="button" onclick="sp.value=(parseFloat(sp.value||0)-0.5).toFixed(1)">-</button>
        <input id="sp" type="number" step="any" value="0.0">
        <button class="trim-btn" type="button" onclick="sp.value=(parseFloat(sp.value||0)+0.5).toFixed(1)">+</button>
      </div>
      <div class="hint">Fine trim over boot angle</div>
    </div>
  </div>
  <div class="row">
    <div class="field"><label>Power Multiplier</label><input id="pm" type="number" step="any" value="1.0"><div class="hint">Scales motor output</div></div>
    <div class="field"><label>Min PWM</label><input id="mpwm" type="number" step="any" value="80"><div class="hint">Overcome friction</div></div>
  </div>
  <div class="row">
    <div class="field"><label>Tilt Gain</label><input id="tg" type="number" step="any" value="1.0"><div class="hint">Extra output boost</div></div>
    <div class="field"><label>Fall Angle &deg;</label><input id="fall" type="number" step="any" value="35.0"><div class="hint">Cut motors beyond this</div></div>
  </div>
  <div class="row">
    <div class="field"><label>Worried Angle &deg;</label><input id="worr" type="number" step="any" value="20.0"><div class="hint">Show X eyes beyond this</div></div>
  </div>
  <button class="save" type="submit">&#x1F4BE; Save &amp; Apply</button>
</form>
<form onsubmit="resetBootAngle();return false;">
  <button class="bootbtn" type="submit">&#x1F504; Reset Boot Angle</button>
</form>
<form onsubmit="resetVals();return false;">
  <button class="reset" type="submit">&#x21BA; Reset to Defaults</button>
</form>
</div>
<script>
function sendVals(){
  let url='/set?kp='+kp.value+'&ki='+ki.value+'&kd='+kd.value
        +'&sp='+sp.value+'&pm='+pm.value+'&mpwm='+mpwm.value
        +'&tg='+tg.value+'&fall='+fall.value+'&worr='+worr.value;
  fetch(url).then(r=>r.text()).then(()=>{
    let m=document.getElementById('savedMsg');
    m.style.display='block'; setTimeout(()=>m.style.display='none',3000);
  });
}
function resetBootAngle(){
  if(!confirm('This will clear the stored Boot Angle.\nReboot the robot after this to remeasure.\n\nContinue?')) return;
  fetch('/resetBootAngle').then(r=>r.text()).then(()=>{
    let m=document.getElementById('bootMsg');
    m.style.display='block'; setTimeout(()=>m.style.display='none',5000);
  });
}
function resetVals(){
  fetch('/reset').then(()=>location.reload());
}
function loadCurrent(){
  fetch('/current').then(r=>r.json()).then(d=>{
    kp.value=d.kp; ki.value=d.ki; kd.value=d.kd; sp.value=d.sp;
    pm.value=d.pm; mpwm.value=d.mpwm; tg.value=d.tg;
    fall.value=d.fall; worr.value=d.worr;
  });
}
setInterval(()=>fetch('/telemetry').then(r=>r.text()).then(t=>document.getElementById('tel').innerText=t),200);
loadCurrent();
</script></body></html>
)rawliteral";

// Web Server handlers
void handleRoot() {
  server.send(200, "text/html", htmlPage);
}

void handleSet() {
  if (server.hasArg("kp"))   Kp              = server.arg("kp").toFloat();
  if (server.hasArg("ki"))   Ki              = server.arg("ki").toFloat();
  if (server.hasArg("kd"))   Kd              = server.arg("kd").toFloat();
  if (server.hasArg("sp"))   setpoint        = server.arg("sp").toFloat();
  if (server.hasArg("pm"))   powerMultiplier = server.arg("pm").toFloat();
  if (server.hasArg("mpwm")) minPWM          = server.arg("mpwm").toInt();
  if (server.hasArg("tg"))   tiltGain        = server.arg("tg").toFloat();
  if (server.hasArg("fall")) fallAngle       = server.arg("fall").toFloat();
  if (server.hasArg("worr")) worriedAngle    = server.arg("worr").toFloat();
  integral = 0.0f;
  prevError = 0.0f;
  savePrefs();
  server.send(200, "text/plain", "OK");
}

void handleReset() {
  resetPrefs();
  server.send(200, "text/plain", "OK");
}

// Clears only the stored boot angle from NVS.
void handleResetBootAngle() {
  clearBootAngle();
  Serial.println("Boot angle cleared from NVS. Reboot robot to remeasure.");
  server.send(200, "text/plain", "Boot angle cleared. Reboot robot to remeasure.");
}

void handleTelemetry() {
  float effectiveTarget = dynamicSetpoint + setpoint;
  String out = "Active Angle: "  + String(angleDeg, 2)       + " deg\n";
  out += "Target Angle: "        + String(effectiveTarget, 2) + " deg\n";
  out += "Boot Angle: "          + String(dynamicSetpoint, 2) + " deg\n";
  out += "Gyro: "                + String(gyroRateDeg, 2)     + " deg/s\n";
  out += "Output: "              + String(lastOutput, 2)      + "\n";
  out += "Kp/Ki/Kd: "           + String(Kp,2)+"/"+String(Ki,2)+"/"+String(Kd,2)+"\n";
  out += "PowerMult: "           + String(powerMultiplier, 2) + "\n";
  out += "MinPWM: "              + String(minPWM)             + "\n";
  out += "TiltGain: "            + String(tiltGain, 2);
  server.send(200, "text/plain", out);
}

void handleCurrent() {
  String json = "{";
  json += "\"kp\":"    + String(Kp, 4)              + ",";
  json += "\"ki\":"    + String(Ki, 4)              + ",";
  json += "\"kd\":"    + String(Kd, 4)              + ",";
  json += "\"sp\":"    + String(setpoint, 4)         + ",";
  json += "\"pm\":"    + String(powerMultiplier, 4)  + ",";
  json += "\"mpwm\":"  + String(minPWM)              + ",";
  json += "\"tg\":"    + String(tiltGain, 4)         + ",";
  json += "\"fall\":"  + String(fallAngle, 4)         + ",";
  json += "\"worr\":"  + String(worriedAngle, 4)     + "}";
  server.send(200, "application/json", json);
}

// Eyes Sequence
void drawX(Adafruit_SSD1306 &disp) {
  disp.clearDisplay();
  int cx = SCREEN_WIDTH / 2;
  int cy = SCREEN_HEIGHT / 2;
  int s  = 22;
  int t  = 6;
  for (int i = -t/2; i <= t/2; i++) {
    disp.drawLine(cx - s + i, cy - s, cx + s + i, cy + s, SSD1306_WHITE);
    disp.drawLine(cx + s + i, cy - s, cx - s + i, cy + s, SSD1306_WHITE);
  }
  disp.display();
}

void setFace() {
  if (faceIsX) return;
  faceIsX = true;
  drawX(displayL);
  drawX(displayR);
}

void SetDefault() {
  if (!faceIsX) return;
  faceIsX = false;
  displayL.clearDisplay(); displayL.display();
  displayR.clearDisplay(); displayR.display();
  roboEyesL.begin(SCREEN_WIDTH, SCREEN_HEIGHT, 100);
  roboEyesL.setCyclops(ON); roboEyesL.setAutoblinker(ON, 2, 3); roboEyesL.setIdleMode(ON, 3, 2);
  roboEyesR.begin(SCREEN_WIDTH, SCREEN_HEIGHT, 100);
  roboEyesR.setCyclops(ON); roboEyesR.setAutoblinker(ON, 2, 3); roboEyesR.setIdleMode(ON, 3, 2);
}

// Motor Control Sequence / Operation
void driveMotors(float cmd) {
  float boostedCmd = cmd * powerMultiplier;
  if (fabsf(boostedCmd) < 80) boostedCmd *= 1.5f;
  int pwm = constrain((int)fabsf(boostedCmd), 0, 255);
  if (pwm > 0 && pwm < minPWM) pwm = minPWM;

  if (boostedCmd > 0) {
    digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
    digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
  } else if (boostedCmd < 0) {
    digitalWrite(IN1, LOW);  digitalWrite(IN2, HIGH);
    digitalWrite(IN3, LOW);  digitalWrite(IN4, HIGH);
  } else {
    digitalWrite(IN1, LOW);  digitalWrite(IN2, LOW);
    digitalWrite(IN3, LOW);  digitalWrite(IN4, LOW);
  }
  ledcWrite(ENA, pwm);
  ledcWrite(ENB, pwm);
}

void motorStop() {
  digitalWrite(IN1, LOW); digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW); digitalWrite(IN4, LOW);
  ledcWrite(ENA, 0);
  ledcWrite(ENB, 0);
}

// Boot angle helper
float measureBootAngle() {
  Serial.println("Warm-up: hold robot still at desired balance angle...");
  for (int s = 5; s >= 1; s--) {
    for (Adafruit_SSD1306 *d : {&displayL, &displayR}) {
      d->clearDisplay();
      d->setTextColor(SSD1306_WHITE);
      d->setTextSize(4); d->setCursor(52, 6);  d->print(s);
      d->setTextSize(1); d->setCursor(34, 52); d->print("HOLD STILL");
      d->display();
    }
    Serial.printf("  %d...\n", s);
    delay(1000);
  }

  Serial.println("Measuring boot angle...");
  sensors_event_t a, g, t;
  float sumAngle = 0.0f;
  for (int i = 0; i < 30; i++) {
    mpu.getEvent(&a, &g, &t);
    sumAngle += -atan2(a.acceleration.x, a.acceleration.z) * 180.0f / PI;
    delay(5);
  }
  float result = sumAngle / 30.0f;
  Serial.printf("Boot angle measured: %.2f deg\n", result);
  return result;
}

// Setup Code
void setup() {
  Serial.begin(115200);
  delay(300);
  Serial.println("\n Self-Balancing Robot ");

  // Set LOW before PWM attaches to prevent boot glitch on GPIO14
  pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);
  digitalWrite(IN1, LOW); digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW); digitalWrite(IN4, LOW);
  ledcAttach(ENA, PWM_FREQ, PWM_RES);
  ledcAttach(ENB, PWM_FREQ, PWM_RES);
  motorStop();

  // Load saved PID values (or defaults if first boot) from NVS
  loadPrefs();

  // I2C Bus 0 for Oled Display
  // SDA=21, SCL=22
  Wire.begin(21, 22);
  Wire.setClock(400000UL);

  // I2C Bus 1 for MPU6050
  // SDA=16, SCL=17
  I2CMPU.begin(16, 17, 400000);

  // Oled Display check the Oled Display
  if (!displayL.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR_L)) {
    Serial.println("Left OLED fail"); while (true) delay(500);
  }
  if (!displayR.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR_R)) {
    Serial.println("Right OLED fail"); while (true) delay(500);
  }
  displayL.setRotation(2);
  displayR.setRotation(2);
  displayL.clearDisplay(); displayL.display();
  displayR.clearDisplay(); displayR.display();

  // Startup message on both OLEDs
  for (Adafruit_SSD1306 *d : {&displayL, &displayR}) {
    d->clearDisplay();
    d->setTextColor(SSD1306_WHITE);
    d->setTextSize(1); d->setCursor(10, 8);  d->println("BalanceBot");
    d->setTextSize(1); d->setCursor(30, 20); d->println("12345678");
    d->setTextSize(1); d->setCursor(4, 38);  d->println("http://192.168.4.1");
    d->display();
  }
  delay(1000);

  // MPU6050 Checking Address
  if (!mpu.begin(0x68, &I2CMPU)) {
    Serial.println("MPU6050 fail"); while (true) delay(500);
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.println("MPU6050 OK");

  // Check if a boot angle was previously measured and saved.
  float storedAngle = loadBootAngle();

  if (storedAngle == BOOT_ANGLE_UNSET) {
    // "Reset Boot Angle" button was pressed. Run full warm-up and measurement.
    Serial.println("No stored boot angle found. Running measurement...");
    dynamicSetpoint = measureBootAngle();
    saveBootAngle(dynamicSetpoint);
    Serial.printf("Boot angle saved to NVS: %.2f deg\n", dynamicSetpoint);

    // Show measured angle on OLEDs
    for (Adafruit_SSD1306 *d : {&displayL, &displayR}) {
      d->clearDisplay();
      d->setTextColor(SSD1306_WHITE);
      d->setTextSize(1); d->setCursor(14, 10); d->print("Balance Target:");
      d->setTextSize(2); d->setCursor(22, 28); d->print(dynamicSetpoint, 1);
      d->print(" deg");
      d->setTextSize(1); d->setCursor(20, 52); d->print("(Saved to flash)");
      d->display();
    }
    delay(2000);

  } else {
    // Stored angle found — skip warm-up and measurement entirely.
    dynamicSetpoint = storedAngle;
    Serial.printf("Boot angle loaded from NVS: %.2f deg (skipping warm-up)\n", dynamicSetpoint);

    // Show loaded angle on OLEDs briefly
    for (Adafruit_SSD1306 *d : {&displayL, &displayR}) {
      d->clearDisplay();
      d->setTextColor(SSD1306_WHITE);
      d->setTextSize(1); d->setCursor(14, 10); d->print("Balance Target:");
      d->setTextSize(2); d->setCursor(22, 28); d->print(dynamicSetpoint, 1);
      d->print(" deg");
      d->setTextSize(1); d->setCursor(22, 52); d->print("(From memory)");
      d->display();
    }
    delay(1500);
  }

  angleDeg = dynamicSetpoint;

  // RoboEyes Display Sequence
  displayL.clearDisplay(); displayL.display();
  displayR.clearDisplay(); displayR.display();
  roboEyesL.begin(SCREEN_WIDTH, SCREEN_HEIGHT, 100);
  roboEyesL.setCyclops(ON); roboEyesL.setAutoblinker(ON, 2, 3); roboEyesL.setIdleMode(ON, 3, 2);
  roboEyesR.begin(SCREEN_WIDTH, SCREEN_HEIGHT, 100);
  roboEyesR.setCyclops(ON); roboEyesR.setAutoblinker(ON, 2, 3); roboEyesR.setIdleMode(ON, 3, 2);
  faceIsX = false;

  // WiFi Access Point
  WiFi.softAP(ssid, password);
  IPAddress ip = WiFi.softAPIP();
  Serial.print("WiFi AP started. Open browser: http://"); Serial.println(ip);

  server.on("/",                handleRoot);
  server.on("/set",             handleSet);
  server.on("/reset",           handleReset);
  server.on("/resetBootAngle",  handleResetBootAngle);
  server.on("/telemetry",       handleTelemetry);
  server.on("/current",         handleCurrent);
  server.begin();
  Serial.println("Web server started.");

  lastMicrosTs = micros();
  Serial.println("\nReady. PID running.");
  Serial.printf("Kp=%.2f Ki=%.2f Kd=%.2f  Target=%.2f deg\n",
                Kp, Ki, Kd, dynamicSetpoint + setpoint);
}

// Loop Code
void loop() {

  server.handleClient();

  // RoboEyes update (Default Eyes Mode)
  if (!faceIsX) {
    roboEyesL.update();
    roboEyesR.update();
  }

  // Time delta
  unsigned long nowUs = micros();
  float dt = (nowUs - lastMicrosTs) / 1e6f;
  if (dt <= 0.0f || dt > 0.5f) dt = 0.001f;
  lastMicrosTs = nowUs;

  // Read the MPU6050 value
  sensors_event_t a, g, t;
  mpu.getEvent(&a, &g, &t);

  float accelAngleDeg = -atan2(a.acceleration.x, a.acceleration.z) * 180.0f / PI;
  gyroRateDeg = g.gyro.y * (180.0f / PI);

  // Complementary filter
  angleDeg = 0.98f * (angleDeg + gyroRateDeg * dt) + 0.02f * accelAngleDeg;

  // SERIAL MONITOR OUTPUT
  // Comment this section
   /*
  Serial.print("Accel_Angle:"); Serial.print(accelAngleDeg);
  Serial.print(",");
  Serial.print("Filtered_Angle:"); Serial.print(angleDeg);
  Serial.print(",");
  Serial.print("Gyro_Rate:"); Serial.println(gyroRateDeg);
   */

  // Effective balance target = boot angle + web setpoint correction
  float effectiveTarget = dynamicSetpoint + setpoint;
  float deviation       = angleDeg - effectiveTarget;

  // Stop the motors if the fall angle is reached and show "X" eyes
  if (fabsf(deviation) > fallAngle) {
    motorStop();
    integral  = 0.0f;
    prevError = 0.0f;
    setFace();
    return;
  }

  // PID Loop Algorithm
  float error = effectiveTarget - angleDeg;
  integral += error * dt;
  integral  = constrain(integral, -200.0f, 200.0f);

  lastOutput = (Kp * error + Ki * integral - Kd * gyroRateDeg) * tiltGain;

  driveMotors(lastOutput);

  // Eyes Sequence
  if (fabsf(deviation) > worriedAngle) {
    setFace();
  } else {
    SetDefault();
  }
}
// End of the Code.