Wheeled Autonomous Learning Terrain Exploring Rover

/*****************************************************************************************
 * Arduino Sketch for a Differential Drive Robot
 * * - Controller: Arduino Uno R4 WiFi
 * - Motor Driver: L298N Dual H-Bridge
 * - Motors: 2x DC Motors with 16 PPR Quadrature Encoders
 * - Control: Moves in inches, rotates in degrees.
 * - Author: Gemini
 * - Date: September 14, 2025
 *****************************************************************************************/

// --- Physical Constants ---
// IMPORTANT: You must measure and update these values for your specific robot!
#define WHEEL_DIAMETER_INCHES 3.5     // Diameter of the robot's wheels in inches
#define ROBOT_TRACK_WIDTH_INCHES 5.0  // Distance between the center of the two wheels in inches
#define ENCODER_PPR 16                // Pulses Per Revolution for the motor encoders

// --- Calculated Constants (Do not change) ---
#define COUNTS_PER_REVOLUTION (ENCODER_PPR * 4) // Quadrature decoding gives 4 counts per pulse
#define WHEEL_CIRCUMFERENCE_INCHES (WHEEL_DIAMETER_INCHES * PI)
#define INCHES_PER_COUNT (WHEEL_CIRCUMFERENCE_INCHES / COUNTS_PER_REVOLUTION)
#define COUNTS_PER_INCH (1.0 / INCHES_PER_COUNT)
#define COUNTS_PER_DEGREE ((ROBOT_TRACK_WIDTH_INCHES * PI / 360.0) * COUNTS_PER_INCH)

// --- Pin Definitions: Left Motor (M1) ---
#define PIN_LEFT_MOTOR_IN1 2      // IN1 on L298N
#define PIN_LEFT_MOTOR_IN2 4      // IN2 on L298N
#define PIN_LEFT_MOTOR_ENABLE 3   // ENA on L298N (must be a PWM pin)

#define PIN_LEFT_ENCODER_A 9      // Encoder Channel A (must be an interrupt pin)
#define PIN_LEFT_ENCODER_B 10      // Encoder Channel B (must be an interrupt pin)

// --- Pin Definitions: Right Motor (M2) ---
#define PIN_RIGHT_MOTOR_IN1 7    // IN3 on L298N
#define PIN_RIGHT_MOTOR_IN2 8    // IN4 on L298N
#define PIN_RIGHT_MOTOR_ENABLE 6  // ENB on L298N (must be a PWM pin)

#define PIN_RIGHT_ENCODER_A 11     // Encoder Channel A (must be an interrupt pin)
#define PIN_RIGHT_ENCODER_B 12    // Encoder Channel B (must be an interrupt pin)

// --- Global Variables ---
// Volatile is required as these are modified by ISRs (Interrupt Service Routines)
volatile long left_encoder_count = 0;
volatile long right_encoder_count = 0;

// ======================================================================================
// SETUP FUNCTION - Runs once at startup
// ======================================================================================
void setup() {
  Serial.begin(115200);
  Serial.println("Differential Drive Robot Initializing...");

  // --- Configure Motor Pins ---
  pinMode(PIN_LEFT_MOTOR_ENABLE, OUTPUT);
  pinMode(PIN_LEFT_MOTOR_IN1, OUTPUT);
  pinMode(PIN_LEFT_MOTOR_IN2, OUTPUT);
  pinMode(PIN_RIGHT_MOTOR_ENABLE, OUTPUT);
  pinMode(PIN_RIGHT_MOTOR_IN1, OUTPUT);
  pinMode(PIN_RIGHT_MOTOR_IN2, OUTPUT);

  // --- Configure Encoder Pins ---
  // Using INPUT_PULLUP is good practice to avoid floating pins if external pull-ups aren't used.
  pinMode(PIN_LEFT_ENCODER_A, INPUT_PULLUP);
  pinMode(PIN_LEFT_ENCODER_B, INPUT_PULLUP);
  pinMode(PIN_RIGHT_ENCODER_A, INPUT_PULLUP);
  pinMode(PIN_RIGHT_ENCODER_B, INPUT_PULLUP);

  // --- Attach Interrupts ---
  // The ISR is triggered whenever the pin state changes (CHANGE).
  attachInterrupt(digitalPinToInterrupt(PIN_LEFT_ENCODER_A), isr_left_encoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(PIN_LEFT_ENCODER_B), isr_left_encoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(PIN_RIGHT_ENCODER_A), isr_right_encoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(PIN_RIGHT_ENCODER_B), isr_right_encoder, CHANGE);
  
  Serial.println("Initialization Complete. Starting Demo...");
  delay(1000);
}


// ======================================================================================
// MAIN LOOP - Runs repeatedly
// ======================================================================================
void loop() {
  Serial.println("\n--- Starting Movement Sequence ---");

  // Move forward 12 inches
  Serial.println("Moving forward 12 inches...");
  move_forward(12.0, 150);
  delay(1000);

  // Turn right 90 degrees
  Serial.println("Turning right 90 degrees...");
  turn_right(90.0, 120);
  delay(1000);

  // Move backward 6 inches
  Serial.println("Moving backward 6 inches...");
  move_backward(6.0, 150);
  delay(1000);

  // Turn left 180 degrees
  Serial.println("Turning left 180 degrees...");
  turn_left(180.0, 120);
  delay(1000);

  Serial.println("Sequence complete. Pausing for 5 seconds...");
  delay(5000);
}


// ======================================================================================
// MOVEMENT FUNCTIONS
// ======================================================================================

/**
 * @brief Moves the robot forward a specified distance.
 * @param distance_inches The distance to travel in inches.
 * @param speed The motor speed (0-255).
 */
void move_forward(float distance_inches, int speed) {
  long target_counts = distance_inches * COUNTS_PER_INCH;
  
  // Reset encoder counts
  left_encoder_count = 0;
  right_encoder_count = 0;

  // Start motors
  set_left_motor_speed(speed);
  set_right_motor_speed(speed);
  
  // Wait until the average distance is reached
  while ((abs(left_encoder_count) + abs(right_encoder_count)) / 2 < target_counts) {
    // You can add monitoring or safety checks here if needed
    delay(1);
  }
  
  stop_motors();
}

/**
 * @brief Moves the robot backward a specified distance.
 * @param distance_inches The distance to travel in inches.
 * @param speed The motor speed (0-255).
 */
void move_backward(float distance_inches, int speed) {
  move_forward(distance_inches, -speed);
}

/**
 * @brief Turns the robot right (clockwise) by a specified angle.
 * @param degrees The angle to turn in degrees.
 * @param speed The motor speed (0-255).
 */
void turn_right(float degrees, int speed) {
  long target_counts = degrees * COUNTS_PER_DEGREE;
  
  // Reset encoder counts
  left_encoder_count = 0;
  right_encoder_count = 0;

  // Start motors in opposite directions
  set_left_motor_speed(speed);
  set_right_motor_speed(-speed);
  
  // Wait until the average turn distance is reached
  while ((abs(left_encoder_count) + abs(right_encoder_count)) / 2 < target_counts) {
    delay(1);
  }
  
  stop_motors();
}

/**
 * @brief Turns the robot left (counter-clockwise) by a specified angle.
 * @param degrees The angle to turn in degrees.
 * @param speed The motor speed (0-255).
 */
void turn_left(float degrees, int speed) {
  turn_right(degrees, -speed);
}


// ======================================================================================
// LOW-LEVEL MOTOR CONTROL
// ======================================================================================

/**
 * @brief Sets the speed and direction of the left motor.
 * @param speed A value from -255 (full reverse) to 255 (full forward).
 */
void set_left_motor_speed(int speed) {
  if (speed >= 0) {
    digitalWrite(PIN_LEFT_MOTOR_IN1, HIGH);
    digitalWrite(PIN_LEFT_MOTOR_IN2, LOW);
    analogWrite(PIN_LEFT_MOTOR_ENABLE, speed);
  } else {
    digitalWrite(PIN_LEFT_MOTOR_IN1, LOW);
    digitalWrite(PIN_LEFT_MOTOR_IN2, HIGH);
    analogWrite(PIN_LEFT_MOTOR_ENABLE, abs(speed));
  }
}

/**
 * @brief Sets the speed and direction of the right motor.
 * @param speed A value from -255 (full reverse) to 255 (full forward).
 */
void set_right_motor_speed(int speed) {
  if (speed >= 0) {
    digitalWrite(PIN_RIGHT_MOTOR_IN1, HIGH);
    digitalWrite(PIN_RIGHT_MOTOR_IN2, LOW);
    analogWrite(PIN_RIGHT_MOTOR_ENABLE, speed);
  } else {
    digitalWrite(PIN_RIGHT_MOTOR_IN1, LOW);
    digitalWrite(PIN_RIGHT_MOTOR_IN2, HIGH);
    analogWrite(PIN_RIGHT_MOTOR_ENABLE, abs(speed));
  }
}

/**
 * @brief Stops both motors.
 */
void stop_motors() {
  set_left_motor_speed(0);
  set_right_motor_speed(0);
}


// ======================================================================================
// INTERRUPT SERVICE ROUTINES (ISRs)
// ======================================================================================

/**
 * @brief ISR for the left motor encoder. Implements full quadrature decoding.
 */
void isr_left_encoder() {
  static byte previous_state = 0;
  byte current_state = (digitalRead(PIN_LEFT_ENCODER_A) << 1) | digitalRead(PIN_LEFT_ENCODER_B);

  // This lookup table determines the direction of rotation.
  // Index is (previous_state << 2) | current_state
  // Values are: 0 (no change), 1 (forward), -1 (reverse), 2 (error)
  static const int8_t lookup_table[] = {
    0, -1, 1, 2, 
    1, 0, 2, -1, 
    -1, 2, 0, 1, 
    2, 1, -1, 0
  };

  int8_t change = lookup_table[(previous_state << 2) | current_state];

  if (change == 1) {
    left_encoder_count++;
  } else if (change == -1) {
    left_encoder_count--;
  }

  previous_state = current_state;
}

/**
 * @brief ISR for the right motor encoder. Implements full quadrature decoding.
 */
void isr_right_encoder() {
  static byte previous_state = 0;
  byte current_state = (digitalRead(PIN_RIGHT_ENCODER_A) << 1) | digitalRead(PIN_RIGHT_ENCODER_B);

  static const int8_t lookup_table[] = {
    0, 1, -1, 2,   // Note: The direction might be opposite the left motor depending on wiring.
    -1, 0, 2, 1,   // If your robot turns when it should go straight, swap the 1 and -1 in this table.
    1, 2, 0, -1, 
    2, -1, 1, 0
  };
  
  int8_t change = lookup_table[(previous_state << 2) | current_state];
  
  if (change == 1) {
    right_encoder_count++;
  } else if (change == -1) {
    right_encoder_count--;
  }
  
  previous_state = current_state;
}