Maze-Solving Robot (MPU6050 + PID Control)

An autonomous maze-solving robot built with Arduino, utilizing three ultrasonic sensors for spatial awareness and an MPU6050 IMU for precise directional control. The robot employs a wall-following algorithm combined with a dual-PID system to maintain stability and accuracy. This robot competed in INSAT Robolympix 4.0 in the autonomous challenge.

📦 Libraries & Initialization

The MPU6050 IMU handles all orientation tracking. These libraries are essential for I2C communication and gyro data processing:

#include <Wire.h>
#include <MPU6050_tockn.h>

MPU6050 mpu6050(Wire);  // Initialize the IMU

🔌 Pin Mapping

Component	Pin	Function
Left Motor	PWM 6 (Fwd), 3 (Bwd)	Locomotion
Right Motor	PWM 5 (Fwd), 9 (Bwd)	Locomotion
Ultrasonic Front	Trig 12 / Echo 13	Obstacle detection
Ultrasonic Left	Trig 4 / Echo 10	Wall detection
Ultrasonic Right	Trig 7 / Echo 8	Wall detection
MPU6050	I2C (SDA/SCL)	Gyroscopic heading
Start Button	2	Calibration & launch
LED	11	Status indicator

📡 Ultrasonic Sensor Reading

Each sensor reading is averaged over multiple samples to reduce noise. The pulseIn() timeout prevents infinite blocking:

long readSensor(int trig, int echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long duration = pulseIn(echo, HIGH, 30000);
  if (duration == 0) return sensorMax;
  long dist = duration * 0.034 / 2;
  return constrain(dist, sensorMin, sensorMax);
}

🎮 Motor Control Functions

Simple abstractions for driving, stopping, and differential movement:

void stopMotors() {
  analogWrite(in1, 0); analogWrite(in2, 0);
  analogWrite(in3, 0); analogWrite(in4, 0);
}

void driveForward(int spd) {
  analogWrite(in1, spd); analogWrite(in2, 0);
  analogWrite(in3, spd); analogWrite(in4, 0);
}

void setMotors(int l, int r) {
  analogWrite(in1, constrain(l, 0, 255)); analogWrite(in2, 0);
  analogWrite(in3, constrain(r, 0, 255)); analogWrite(in4, 0);
}

🧠 PID Control — Straight Line Walking

When moving forward, the robot uses a full PID loop to ensure perfectly straight travel, correcting for motor inconsistencies or physical bumps.

void pidStep() {
  mpu6050.update();
  float error = sleft() - sright();  // Keep centered between walls
  integral += error;
  integral = constrain(integral, -100, 100);
  derivative = error - preverror;
  float correction = Kp * error + Ki * integral + Kd * derivative;
  int l = constrain(basespeed + correction, minspeed, maxspeed);
  int r = constrain(basespeed - correction, minspeed, maxspeed);
  setMotors(l, r);
  preverror = error;
}

🔄 Pivot PID — Precise Turns

Precise 90° turns are difficult with simple timers due to battery voltage fluctuations. This PD controller handles it:

void pivotToAngle(float target_angle) {
  t = millis();
  while (true) {
    mpu6050.update();
    float current_yaw = mpu6050.getAngleZ();
    float turn_error = target_angle - current_yaw;
    
    if (abs(turn_error) < 5) { stopMotors(); break; }
    if (millis() - t > 2500) { break; }
    
    float turn_diff = turn_error - last_turn_error;
    float turn_correction = (p_turn * turn_error) + (d_turn * turn_diff);
    last_turn_error = turn_error;
    int motor_pwm = constrain(turn_base_speed + abs(turn_correction), 0, max_pwm);
    
    if (turn_error > 0) {
      // Turn LEFT
      analogWrite(in1, motor_pwm); analogWrite(in2, 0);
      analogWrite(in3, 0); analogWrite(in4, motor_pwm);
    } else {
      // Turn RIGHT
      analogWrite(in1, 0); analogWrite(in2, motor_pwm);
      analogWrite(in3, motor_pwm); analogWrite(in4, 0);
    }
  }
  tangle = target_angle;
  integral = 0; preverror = 0;
}

↪️ U-Turn Function

Two consecutive 78° pivots (since 90° was overshooting in testing):

void uTurn() {
  pivotToAngle(78);
  delay(100);
  pivotToAngle(78);
}

🏃 The Algorithm — Right-Hand Rule

The robot implements the Right-Hand Rule (wall follower). This is an "uninformed" maze-solving algorithm where the robot keeps the right wall at a constant relative distance.

Decision Priority:

1. Turn Right: If right sensor detects an opening (dist > 24cm), move forward slightly, then execute -79° pivot.
2. Drive Forward: If right turn not possible but front path clear, continue forward using PID stabilization.
3. Turn Left: If right AND front blocked but left open, execute +79° pivot.
4. U-Turn (Dead End): If all three sides blocked, perform ~165° pivot to backtrack.

void loop() {
  pidStep();
  if(right()){
    driveForward(basespeed);
    delay(50); stopMotors(); delay(150);
    pivotToAngle(tangle - 79);
    stopMotors(); delay(150);
    // Drive into new cell
    t = millis();
    while(millis() - t < 650) { pidStep(); }
  }
  else if(left() && !forward() && !right()){
    driveForward(basespeed);
    delay(50); stopMotors(); delay(150);
    pivotToAngle(tangle + 79);
    stopMotors(); delay(100);
    t = millis();
    while(millis() - t < 650) { pidStep(); }
  }
  else if(!left() && !forward() && !right()){
    pivotToAngle(tangle - 165);  // U-turn
    delay(50);
  }
}

⚙️ PID Tuning Parameters

These values were calibrated specifically for the robot's weight, motor response, and maze surface:

// Walking PID
float Kp = 15;
float Ki = 0.05;
float Kd = 0.1;

// Turn PID
float p_turn = 40.0; 
float d_turn = 3;

// Speed limits
int basespeed = 180;
int maxspeed  = 255;
int minspeed  = 100;
int turn_base_speed = 30;
int max_pwm = 70;

🚀 Setup & Calibration

On power-up, the robot waits for the start button. During this time, the MPU6050 calculates gyro offsets — the robot must stay perfectly still. The LED blinks to indicate waiting state:

void setup() {
  Wire.begin();
  mpu6050.begin();
  // ... pinModes ...
  
  mpu6050.calcGyroOffsets(true);
  
  while (digitalRead(startButton) == LOW) {
    digitalWrite(led, (millis() / 500) % 2);  // Blink LED
    delay(50);
  }
}

🎯 Key Takeaways

• Dual PID systems — one for straight-line stability, one for precise pivots — compensate for hardware imperfections.
• The Right-Hand Rule guarantees maze solving for simply-connected mazes (no loops).
• Gyro-based turning is battery-agnostic — unlike timer-based turns, it works the same at full or low battery.
• Sensor averaging (READINGS = 4) filters out ultrasonic noise common in reflective maze walls.

---

🏆 Competed in INSAT Robolympix 4.0 — Autonomous Challenge

This project taught me more about control theory than any textbook ever could. Breaking the PID loop, watching the robot oscillate, tuning Kp until it screamed down the straightaway — that's real engineering.