📚 Detailed Project Documentation

1. Main Application: Path Tracking (path_tracking_lookahead.py)

Description

This is the flagship script of the repository. It integrates the Maze Generator, Path Planner, and Physics Robot into a single autonomous loop. The robot navigates from a random Start point to a Goal point while dynamically avoiding obstacles added by the user.

Key Classes

• HolonomicPurePursuit: The core controller class.
• Logic: Calculates the desired velocity vector to reach a "lookahead" point on the path.
• Dynamic Avoidance: Calculates a secondary repulsion vector based on the gradient of the Cost Map (Heatmap) to push the robot away from sudden obstacles.

How to Run

python path_tracking_lookahead.py

Controls

• Left Click: Add a dynamic wall/obstacle at the mouse cursor. The robot will immediately re-plan its path.
• R: Reset the simulation (New Maze, New Start/Goal, New Path).

Technical Flow

1. Init: Generates Maze → Calculates Cost Map → Plans Path.
2. Loop:
   • Robot gets position from Physics Engine.
   • Controller calculates command vector (V_x, V_y).
   • Physics Engine applies forces to wheels to achieve (V_x, V_y).
   • If User Clicks: Adds wall rect to list → Recalculates Cost Map → Updates Controller.

2. Physics Engine (d2.py)

Description

A high-fidelity simulator that models a robot with mass, inertia, and friction. Unlike simple geometric simulations, this robot will "drift" if it turns too fast and takes time to accelerate.

Key Classes

• DynamicModule: Represents a single swerve module.
  • PID Control: Uses a Proportional-Integral-Derivative controller to steer the wheel to the correct angle.
  • Friction: Simulates static friction (stiction) and kinetic friction.
• DynamicRobot: The chassis object.
  • Integration: Sums forces from all 4 wheels, applies Drag and Friction, and calculates acceleration (F=ma).

3. Kinematics Engine (k2.py)

Description

Handles the geometric math required to translate chassis movement into individual wheel movements and vice versa. This module is stateless and purely mathematical.

Key Functions

• inverse_kinematics(vx, vy, omega):
  • Takes desired chassis velocity (V_x, V_y) and angular velocity (ω).
  • Returns the target Speed and Angle for each of the 4 swerve modules.
  • Normalizes wheel speeds if any command exceeds the physical maximum of the motor.
• forward_kinematics(module_states):
  • Takes the current speed and angle of the 4 modules.
  • Returns the estimated chassis velocity (V_x, V_y, ω).
  • Used for Odometry calculations to estimate robot position on the field.

4. Path Planner (planner.py)

Description

Responsible for finding the optimal route from Point A to Point B within the grid. It works closely with the Cost Map generated by the grid simulator.

Key Algorithms

• A* (A-Star) Search: The primary pathfinding algorithm. It uses a heuristic (Euclidean distance) to find the shortest path efficiently.
• Path Smoothing:
  • Raw A* output produces "jagged" grid-based paths.
  • This module includes a smoothing pass to reduce unnecessary waypoints, creating straight lines where possible.
• Node Class: Represents a specific (x, y) coordinate, storing its g_cost (distance from start), h_cost (distance to end), and parent (for retracing the path).

5. Grid & Maze Generator (sim_grid.py)

Description

Manages the environment data, including the binary obstacle map (Wall vs. Empty) and the gradient cost map (Heatmap) used for safety.

Key Features

• Maze Generation: Uses a Randomized Depth-First Search (Recursive Backtracker) to generate perfect mazes with no loops.
• Cost Map / Heatmap:
  • Uses a Distance Transform algorithm.
  • Cells near walls have high "cost" values; cells far from walls have low values.
  • This allows the path_tracking controller to "surf" the gradient valleys, naturally staying away from walls.
• Dynamic Updates: When the user clicks to add a wall, this module updates only the affected regions of the grid to maintain performance.

6. Configuration (robot_config.py)

Description

A central file for tuning simulation parameters. Changing values here affects physics, kinematics, and graphics instantly.

Parameters

• Physical Constants:
  • ROBOT_MASS: Total mass in kg.
  • WHEEL_BASE: Width/Length of the drive base.
  • MAX_SPEED: Physical top speed of the motors.
• Control Loop:
  • DT: Time step for the physics integrator (e.g., 0.02s).
  • PID_GAINS: Tuning values (k_P, k_I, k_D) for the module turning motors.
• Visualization:
  • SCREEN_WIDTH / SCREEN_HEIGHT: Pygame window dimensions.
  • CELL_SIZE: Pixel size of grid blocks.