Self-Balancing Line-Following Robot

#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_ADS1X15.h>


// Forward declarations
void IRAM_ATTR leftEncoderISR();
void IRAM_ATTR rightEncoderISR();
bool writeMPU(uint8_t reg, uint8_t value);
bool readMPU(int16_t &ax, int16_t &ay, int16_t &az, int16_t &gx, int16_t &gy, int16_t &gz);
void setOneMotor(uint8_t in1, uint8_t in2, uint8_t pwmPin, int command);
void stopMotors();
void updateLineReading();

constexpr uint8_t IMU_SDA = 21;
constexpr uint8_t IMU_SCL = 22;
constexpr uint8_t LEFT_IN1 = 4;
constexpr uint8_t LEFT_IN2 = 13;
constexpr uint8_t LEFT_PWM = 14;
constexpr uint8_t RIGHT_IN1 = 16;
constexpr uint8_t RIGHT_IN2 = 17;
constexpr uint8_t RIGHT_PWM = 18;
constexpr uint8_t DRIVER_STBY = 19;
constexpr uint8_t LEFT_ENC_A = 23;
constexpr uint8_t LEFT_ENC_B = 25;
constexpr uint8_t RIGHT_ENC_A = 26;
constexpr uint8_t RIGHT_ENC_B = 27;
constexpr uint8_t MPU_ADDRESS = 0x68;
constexpr uint32_t CONTROL_PERIOD_US = 4000;
constexpr uint32_t PWM_FREQUENCY = 20000;
constexpr uint8_t PWM_RESOLUTION = 10;
constexpr int PWM_MAX = 1023;

// Starting gains only: tune with the robot supported, then carefully on the floor.
float balanceKp = 33.0f;
float balanceKi = 0.90f;
float balanceKd = 1.15f;
float velocityKp = 0.20f;
float lineSteerGain = 0.018f;
float uprightTrimDeg = 0.0f;

Adafruit_ADS1115 lineAdcLeft;
Adafruit_ADS1115 lineAdcRight;
bool lineAdcsReady = false;
int16_t lineRaw[8] = {0};
uint8_t lineChannel = 0;
float linePosition = 0.0f;
volatile int32_t leftEncoderCount = 0;
volatile int32_t rightEncoderCount = 0;
int32_t lastLeftEncoderCount = 0;
int32_t lastRightEncoderCount = 0;
float tiltDeg = 0.0f;
float integralError = 0.0f;
uint32_t lastControlUs = 0;

void IRAM_ATTR leftEncoderISR() {
  leftEncoderCount += (digitalRead(LEFT_ENC_A) == digitalRead(LEFT_ENC_B)) ? 1 : -1;
}
void IRAM_ATTR rightEncoderISR() {
  rightEncoderCount += (digitalRead(RIGHT_ENC_A) == digitalRead(RIGHT_ENC_B)) ? 1 : -1;
}

bool writeMPU(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(reg);
  Wire.write(value);
  return Wire.endTransmission() == 0;
}

bool readMPU(int16_t &ax, int16_t &ay, int16_t &az, int16_t &gx, int16_t &gy, int16_t &gz) {
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x3B);
  if (Wire.endTransmission(false) != 0 || Wire.requestFrom(MPU_ADDRESS, (uint8_t)14) != 14) return false;
  ax = (Wire.read() << 8) | Wire.read(); ay = (Wire.read() << 8) | Wire.read(); az = (Wire.read() << 8) | Wire.read();
  Wire.read(); Wire.read();
  gx = (Wire.read() << 8) | Wire.read(); gy = (Wire.read() << 8) | Wire.read(); gz = (Wire.read() << 8) | Wire.read();
  return true;
}

void setOneMotor(uint8_t in1, uint8_t in2, uint8_t pwmPin, int command) {
  command = constrain(command, -PWM_MAX, PWM_MAX);
  digitalWrite(in1, command > 0 ? HIGH : LOW);
  digitalWrite(in2, command < 0 ? HIGH : LOW);
  ledcWrite(pwmPin, abs(command));
}
void stopMotors() {
  setOneMotor(LEFT_IN1, LEFT_IN2, LEFT_PWM, 0);
  setOneMotor(RIGHT_IN1, RIGHT_IN2, RIGHT_PWM, 0);
}

// Read only one QTR channel each balance pass. A complete eight-sensor position is refreshed about 31 times per second.
void updateLineReading() {
  if (!lineAdcsReady) return;
  Adafruit_ADS1115 &adc = (lineChannel < 4) ? lineAdcLeft : lineAdcRight;
  uint8_t input = lineChannel & 0x03;
  lineRaw[lineChannel] = adc.readADC_SingleEnded(input);
  lineChannel++;
  if (lineChannel < 8) return;
  lineChannel = 0;
  const int weights[8] = {-3500, -2500, -1500, -500, 500, 1500, 2500, 3500};
  int32_t total = 0;
  int32_t weighted = 0;
  for (uint8_t i = 0; i < 8; ++i) {
    // Calibrate LINE_WHITE and LINE_BLACK after observing values on your own track.
    int strength = constrain(26000 - lineRaw[i], 0, 26000);
    total += strength;
    weighted += strength * weights[i];
  }
  if (total > 4000) linePosition = (float)weighted / total;
}

void setup() {
  Serial.begin(115200);
  Wire.begin(IMU_SDA, IMU_SCL);
  Wire.setClock(400000);
  pinMode(LEFT_IN1, OUTPUT); pinMode(LEFT_IN2, OUTPUT); pinMode(RIGHT_IN1, OUTPUT); pinMode(RIGHT_IN2, OUTPUT);
  pinMode(DRIVER_STBY, OUTPUT); digitalWrite(DRIVER_STBY, LOW);
  ledcAttach(LEFT_PWM, PWM_FREQUENCY, PWM_RESOLUTION); ledcAttach(RIGHT_PWM, PWM_FREQUENCY, PWM_RESOLUTION);
  stopMotors();
  pinMode(LEFT_ENC_A, INPUT_PULLUP); pinMode(LEFT_ENC_B, INPUT_PULLUP);
  pinMode(RIGHT_ENC_A, INPUT_PULLUP); pinMode(RIGHT_ENC_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(LEFT_ENC_A), leftEncoderISR, CHANGE);
  attachInterrupt(digitalPinToInterrupt(RIGHT_ENC_A), rightEncoderISR, CHANGE);

  lineAdcsReady = lineAdcLeft.begin(0x48, &Wire) && lineAdcRight.begin(0x49, &Wire);
  if (lineAdcsReady) {
    lineAdcLeft.setDataRate(RATE_ADS1115_860SPS);
    lineAdcRight.setDataRate(RATE_ADS1115_860SPS);
  } else Serial.println("QTR ADC boards not found; line correction disabled.");

  writeMPU(0x6B, 0x00); writeMPU(0x1B, 0x00); writeMPU(0x1C, 0x00);
  delay(100);
  int16_t ax, ay, az, gx, gy, gz;
  if (readMPU(ax, ay, az, gx, gy, gz)) {
    tiltDeg = atan2f((float)ax, (float)az) * 180.0f / PI;
    digitalWrite(DRIVER_STBY, HIGH);
    Serial.println("Balancing controller armed. Hold upright, then release gently.");
  } else Serial.println("MPU-6050 not found: motors remain disabled.");
  lastControlUs = micros();
}

void loop() {
  uint32_t now = micros();
  if ((uint32_t)(now - lastControlUs) < CONTROL_PERIOD_US) return;
  float dt = (float)(now - lastControlUs) / 1000000.0f;
  lastControlUs = now;
  int16_t ax, ay, az, gx, gy, gz;
  if (!readMPU(ax, ay, az, gx, gy, gz)) { digitalWrite(DRIVER_STBY, LOW); stopMotors(); return; }
  float accelTiltDeg = atan2f((float)ax, (float)az) * 180.0f / PI;
  float gyroRateDegPerSec = (float)gy / 131.0f;
  tiltDeg = 0.98f * (tiltDeg + gyroRateDegPerSec * dt) + 0.02f * accelTiltDeg;
  updateLineReading();
  noInterrupts(); int32_t leftNow = leftEncoderCount, rightNow = rightEncoderCount; interrupts();
  float wheelSpeed = ((leftNow - lastLeftEncoderCount) + (rightNow - lastRightEncoderCount)) / (2.0f * dt);
  lastLeftEncoderCount = leftNow; lastRightEncoderCount = rightNow;
  float error = tiltDeg - uprightTrimDeg;
  integralError = constrain(integralError + error * dt, -80.0f, 80.0f);
  float balanceCommand = balanceKp * error + balanceKi * integralError + balanceKd * gyroRateDegPerSec + velocityKp * wheelSpeed;
  if (fabsf(tiltDeg) > 35.0f) { integralError = 0.0f; digitalWrite(DRIVER_STBY, LOW); stopMotors(); return; }
  float steering = linePosition * lineSteerGain;
  digitalWrite(DRIVER_STBY, HIGH);
  setOneMotor(LEFT_IN1, LEFT_IN2, LEFT_PWM, (int)(balanceCommand - steering));
  setOneMotor(RIGHT_IN1, RIGHT_IN2, RIGHT_PWM, (int)(balanceCommand + steering));
}