Feature/pure pursuit

[vvictorrr]

wrote pure pursuit, based off existing code, adapted for our codebase

#18

[Copilot]

Pull Request Overview

This PR introduces a Pure Pursuit path following controller for autonomous robot navigation. The implementation provides an asynchronous controller that computes motor velocities based on lookahead points along a predefined path.

Key changes:

• Adds AsyncAdaptivePurePursuitController class with path loading, lookahead point calculation, and curvature-based control
• Integrates the controller with existing tank drive and odometry systems
• Supports loading paths from CSV files

Reviewed Changes

Copilot reviewed 4 out of 4 changed files in this pull request and generated 5 comments.

File	Description
src/purepusuit.cpp	Implements the Pure Pursuit controller with path following logic and control loop
include/purepursuit.h	Declares the controller class, PathPoint structure, and public/private interfaces
include/subsystems.h	Adds purepursuit.h to subsystems includes
include/main.h	Adds purepursuit.h to main includes

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

Using a magic number 1e9 as an initial distance is unclear and could overflow for certain coordinate systems. Use std::numeric_limits<double>::max() from <limits> or std::numeric_limits<double>::infinity() for a clearer and more robust implementation.

[Copilot]

The magic number 5 is hardcoded for lookahead index offset. This should either be calculated based on lookaheadDist parameter (which is currently unused), or extracted as a named constant to clarify its purpose and make it configurable.

Suggested change

	int lookaheadIdx = std::min(nearestIdx + 5, (int)path.size() - 1);
	// Find the first point at least lookaheadDist away from the robot pose, starting from nearestIdx
	int lookaheadIdx = nearestIdx;
	double accumDist = 0.0;
	for (int i = nearestIdx; i < static_cast<int>(path.size()) - 1; ++i) {
	double dx = path[i + 1].x - path[i].x;
	double dy = path[i + 1].y - path[i].y;
	accumDist += std::sqrt(dx * dx + dy * dy);
	if (accumDist >= lookaheadDist) {
	lookaheadIdx = i + 1;
	break;
	}
	}
	// Clamp to last point if we run out of path
	lookaheadIdx = std::min(lookaheadIdx, static_cast<int>(path.size()) - 1);

[Copilot]

Forward declaration would be sufficient here since the class only stores a pointer to TankDrive. Replace the include with class TankDrive; to reduce compilation dependencies and improve build times.

Suggested change

	#include "tank_drive.h"
	class TankDrive;