Feature/pure pursuit

include/main.h

@@ -42,6 +42,7 @@
 #include "subsystems.h"
 #include "tank_drive.h"
 #include "transport.h"
+#include "purepursuit.h"
 
 /**
  * You should add more #includes here

include/purepursuit.h

@@ -0,0 +1,56 @@
+#ifndef PURE_PURSUIT_H
+#define PURE_PURSUIT_H
+
+#include <vector>
+#include <string>
+#include <memory>
+#include <atomic>
+#include <thread>
+#include <fstream>
+#include <sstream>
+#include <cmath>
+#include <iostream>
+
+#include "tank_drive.h" 
+#include "odometry.h"     // needs to provide pose {x, y, theta}
+
+// Structure for a single path point
+struct PathPoint {
+    double x;
+    double y;
+    double heading;
+    double velocity;
+};
+
+// Asynchronous Adaptive Pure Pursuit Controller
+class AsyncAdaptivePurePursuitController {
+public:
+    AsyncAdaptivePurePursuitController(TankDrive* drive, std::shared_ptr<Odometry> odom);
+    ~AsyncAdaptivePurePursuitController();
+
+    // Path management
+    void setPath(const std::vector<PathPoint>& newPath);
+    void setPathFromCSV(const std::string& filename);
+
+    // Control
+    void start();
+    void stop();
+    bool isRunning() const;
+
+private:
+    TankDrive* drive;
+    std::shared_ptr<Odometry> odom;
+    std::vector<PathPoint> path;
+
+    std::atomic<bool> running;
+    std::thread controlThread;
+
+    // Internal helpers 
+    void runLoop();
+    std::vector<PathPoint> loadPathFromCSV(const std::string& filename);
+    PathPoint findLookaheadPoint(const Pose& robotPose, double lookaheadDist);
+    double computeCurvature(const Pose& robotPose, const PathPoint& target);
+    double clamp(double value, double minVal, double maxVal);
+};
+
+#endif

include/subsystems.h

@@ -11,6 +11,7 @@
 #include "robot_config.h"
 #include "tank_drive.h"
 #include "transport.h"
+#include "purepursuit.h"
 
 /**
  * Subsystems are initialized here. All subsystems should take a configuration

src/purepusuit.cpp

@@ -0,0 +1,133 @@
+#include "purepursuit.h"
+#include <iostream>
+
+AsyncAdaptivePurePursuitController::AsyncAdaptivePurePursuitController(TankDrive* drive, std::shared_ptr<Odometry> odom)
+    : drive(drive), odom(odom) {}
+
+AsyncAdaptivePurePursuitController::~AsyncAdaptivePurePursuitController() {
+    stop();
+}
+
+void AsyncAdaptivePurePursuitController::setPath(const std::vector<PathPoint>& newPath) {
+    path = newPath;
+}
+
+void AsyncAdaptivePurePursuitController::setPathFromCSV(const std::string& filename) {
+    path = loadPathFromCSV(filename);
+}
+
+bool AsyncAdaptivePurePursuitController::isRunning() const {
+    return running.load();
+}
+
+void AsyncAdaptivePurePursuitController::start() {
+    if (running) return;
+    running = true;
+    controlThread = std::thread(&AsyncAdaptivePurePursuitController::runLoop, this);
+}
+
+void AsyncAdaptivePurePursuitController::stop() {
+    running = false;
+    if (controlThread.joinable()) controlThread.join();
+}
+
+std::vector<PathPoint> AsyncAdaptivePurePursuitController::loadPathFromCSV(const std::string& filename) {
+    std::vector<PathPoint> path;
+    std::ifstream file(filename);
+    std::string line;
+
+    if (!file.is_open()) {
+        std::cerr << "[PurePursuit] Failed to open CSV file: " << filename << std::endl;
+        return path;
+    }
+
+    while (std::getline(file, line)) {
+        std::stringstream ss(line);
+        std::string xStr, yStr;
+
+        if (!std::getline(ss, xStr, ',')) continue;
+        if (!std::getline(ss, yStr, ',')) continue;
+
+        double x = std::stod(xStr);
+        double y = std::stod(yStr);
+
+        path.push_back({x, y, 0.0, 0.0});
+    }
+
+    // Compute headings
+    for (size_t i = 0; i + 1 < path.size(); ++i) {
+        double dx = path[i + 1].x - path[i].x;
+        double dy = path[i + 1].y - path[i].y;
+        path[i].heading = std::atan2(dy, dx);
+    }
+
+    // Assign velocities
+    for (auto &p : path)
+        p.velocity = 1.0;  // 1 m/s default, tune later
+
+    std::cout << "[PurePursuit] Loaded path with " << path.size() << " points." << std::endl;
+    return path;
+}
+
+PathPoint AsyncAdaptivePurePursuitController::findLookaheadPoint(const Pose& robotPose, double lookaheadDist) {
+    if (path.empty()) return {0, 0, 0, 0};
+
+    // Simple nearest-point + lookahead approach
+    double minDist = 1e9;
+    int nearestIdx = 0;
+
+    for (int i = 0; i < static_cast<int>(path.size()); ++i) {
+        double dx = path[i].x - robotPose.x;
+        double dy = path[i].y - robotPose.y;
+        double dist = std::sqrt(dx * dx + dy * dy);
+        if (dist < minDist) {
+            minDist = dist;
+            nearestIdx = i;
+        }
+    }
+
+    int lookaheadIdx = std::min(nearestIdx + 5, (int)path.size() - 1);
+    return path[lookaheadIdx];
+}
+
+double AsyncAdaptivePurePursuitController::computeCurvature(const Pose& robotPose, const PathPoint& target) {
+    double dx = target.x - robotPose.x;
+    double dy = target.y - robotPose.y;
+    double distance2 = dx * dx + dy * dy;
+    double headingError = std::atan2(dy, dx) - robotPose.theta;
+    return (2.0 * std::sin(headingError)) / std::sqrt(distance2);
+}
+
+double AsyncAdaptivePurePursuitController::clamp(double value, double minVal, double maxVal) {
+    if (value < minVal) return minVal;
+    if (value > maxVal) return maxVal;
+    return value;
+}
+
+void AsyncAdaptivePurePursuitController::runLoop() {
+    const double lookaheadDist = 0.25;  // tune this
+    const double baseSpeed = 0.5;       // tune this
+    const double trackWidth = 0.3;      // tune this
+
+    std::cout << "[PurePursuit] Starting control loop..." << std::endl;
+
+    while (running) {
+        Pose pose;
+        odom->getPose(&pose);
+        PathPoint target = findLookaheadPoint(pose, lookaheadDist);
+
+        double curvature = computeCurvature(pose, target);
+        double leftVel = baseSpeed * (2.0 - curvature * trackWidth) / 2.0;
+        double rightVel = baseSpeed * (2.0 + curvature * trackWidth) / 2.0;
+
+        leftVel = clamp(leftVel, -1.0, 1.0);
+        rightVel = clamp(rightVel, -1.0, 1.0);
+
+        drive->tank(leftVel, rightVel);
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(20));
+    }
+
+    drive->tank(0, 0);
+    std::cout << "[PurePursuit] Stopped." << std::endl;
+}