From 28a21e7e80b035a49b9aaf1dea5fb14db87f5992 Mon Sep 17 00:00:00 2001 From: victorli Date: Wed, 29 Oct 2025 11:36:09 -0400 Subject: [PATCH 1/6] purepursuit first attempts --- include/purepursuit.h | 56 ++++++++++++++++ src/purepusuit.cpp | 149 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 205 insertions(+) create mode 100644 include/purepursuit.h create mode 100644 src/purepusuit.cpp diff --git a/include/purepursuit.h b/include/purepursuit.h new file mode 100644 index 0000000..3d9f0a8 --- /dev/null +++ b/include/purepursuit.h @@ -0,0 +1,56 @@ +#ifndef PURE_PURSUIT_H +#define PURE_PURSUIT_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "tank_drive.h" // Your tank drive interface +#include "odometry.h" // Provides 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 odom); + ~AsyncAdaptivePurePursuitController(); + + // Path management + void setPath(const std::vector& newPath); + void setPathFromCSV(const std::string& filename); + + // Control + void start(); + void stop(); + bool isRunning() const; + +private: + TankDrive* drive; + std::shared_ptr odom; + std::vector path; + + std::atomic running; + std::thread controlThread; + + // --- Internal helpers --- + void runLoop(); + std::vector 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 \ No newline at end of file diff --git a/src/purepusuit.cpp b/src/purepusuit.cpp new file mode 100644 index 0000000..80ba75c --- /dev/null +++ b/src/purepusuit.cpp @@ -0,0 +1,149 @@ +#include "AsyncAdaptivePurePursuitController.hpp" +#include + +AsyncAdaptivePurePursuitController::AsyncAdaptivePurePursuitController(TankDrive* drive, std::shared_ptr odom) + : drive(drive), odom(odom) {} + +AsyncAdaptivePurePursuitController::~AsyncAdaptivePurePursuitController() { + stop(); +} + +void AsyncAdaptivePurePursuitController::setPath(const std::vector& 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 AsyncAdaptivePurePursuitController::loadPathFromCSV(const std::string& filename) { + std::vector 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 nominal 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(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; // meters, tune this + const double baseSpeed = 0.5; // m/s, tune for your robot + const double trackWidth = 0.3; // meters, tune for your drivetrain + + std::cout << "[PurePursuit] Starting control loop..." << std::endl; + + while (running) { + Pose pose = odom->getPose(); + 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; +} +🧭 Example usage +cpp +Copy code +#include "AsyncAdaptivePurePursuitController.hpp" + +int main() { + TankDrive drive; // your implementation + auto odom = std::make_shared(); + + AsyncAdaptivePurePursuitController purePursuit(&drive, odom); + purePursuit.setPathFromCSV("/usd/test_path.csv"); + purePursuit.start(); + + // Run for ~10 seconds + pros::delay(10000); + purePursuit.stop(); +} \ No newline at end of file From 870115feb1dccaae6683a1abdcef6e5ecbd65aff Mon Sep 17 00:00:00 2001 From: victorli Date: Wed, 29 Oct 2025 16:01:39 -0400 Subject: [PATCH 2/6] inclusions in subsystems and main --- include/main.h | 1 + include/subsystems.h | 1 + 2 files changed, 2 insertions(+) diff --git a/include/main.h b/include/main.h index 66eaaab..74310fe 100644 --- a/include/main.h +++ b/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 diff --git a/include/subsystems.h b/include/subsystems.h index 7d190cc..2d6ec7f 100644 --- a/include/subsystems.h +++ b/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 From 408e0e904375718659036f2e5aabbd7f05239ea0 Mon Sep 17 00:00:00 2001 From: victorli Date: Wed, 29 Oct 2025 20:33:42 -0400 Subject: [PATCH 3/6] edit comments --- include/purepursuit.h | 6 +++--- src/purepusuit.cpp | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/include/purepursuit.h b/include/purepursuit.h index 3d9f0a8..e87cf5a 100644 --- a/include/purepursuit.h +++ b/include/purepursuit.h @@ -11,8 +11,8 @@ #include #include -#include "tank_drive.h" // Your tank drive interface -#include "odometry.h" // Provides Pose {x, y, theta} +#include "tank_drive.h" +#include "odometry.h" // needs to provide pose {x, y, theta} // Structure for a single path point struct PathPoint { @@ -45,7 +45,7 @@ class AsyncAdaptivePurePursuitController { std::atomic running; std::thread controlThread; - // --- Internal helpers --- + // Internal helpers void runLoop(); std::vector loadPathFromCSV(const std::string& filename); PathPoint findLookaheadPoint(const Pose& robotPose, double lookaheadDist); diff --git a/src/purepusuit.cpp b/src/purepusuit.cpp index 80ba75c..b35d3ed 100644 --- a/src/purepusuit.cpp +++ b/src/purepusuit.cpp @@ -61,7 +61,7 @@ std::vector AsyncAdaptivePurePursuitController::loadPathFromCSV(const path[i].heading = std::atan2(dy, dx); } - // Assign nominal velocities + // Assign velocities for (auto &p : path) p.velocity = 1.0; // 1 m/s default, tune later @@ -105,9 +105,9 @@ double AsyncAdaptivePurePursuitController::clamp(double value, double minVal, do } void AsyncAdaptivePurePursuitController::runLoop() { - const double lookaheadDist = 0.25; // meters, tune this - const double baseSpeed = 0.5; // m/s, tune for your robot - const double trackWidth = 0.3; // meters, tune for your drivetrain + 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; From 05c5cd6f00d93eb1ec465240038bbbb4865192ea Mon Sep 17 00:00:00 2001 From: victorli Date: Wed, 29 Oct 2025 20:36:25 -0400 Subject: [PATCH 4/6] small fix --- src/purepusuit.cpp | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/src/purepusuit.cpp b/src/purepusuit.cpp index b35d3ed..d50b27a 100644 --- a/src/purepusuit.cpp +++ b/src/purepusuit.cpp @@ -129,21 +129,4 @@ void AsyncAdaptivePurePursuitController::runLoop() { drive->tank(0, 0); std::cout << "[PurePursuit] Stopped." << std::endl; -} -🧭 Example usage -cpp -Copy code -#include "AsyncAdaptivePurePursuitController.hpp" - -int main() { - TankDrive drive; // your implementation - auto odom = std::make_shared(); - - AsyncAdaptivePurePursuitController purePursuit(&drive, odom); - purePursuit.setPathFromCSV("/usd/test_path.csv"); - purePursuit.start(); - - // Run for ~10 seconds - pros::delay(10000); - purePursuit.stop(); } \ No newline at end of file From 87ccdeddf9c457773d43b7eb235a2f95cb00f135 Mon Sep 17 00:00:00 2001 From: vvictorrr <70972941+vvictorrr@users.noreply.github.com> Date: Mon, 10 Nov 2025 15:26:44 -0500 Subject: [PATCH 5/6] Update src/purepusuit.cpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/purepusuit.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/purepusuit.cpp b/src/purepusuit.cpp index d50b27a..e7d7ad0 100644 --- a/src/purepusuit.cpp +++ b/src/purepusuit.cpp @@ -1,4 +1,4 @@ -#include "AsyncAdaptivePurePursuitController.hpp" +#include "purepursuit.h" #include AsyncAdaptivePurePursuitController::AsyncAdaptivePurePursuitController(TankDrive* drive, std::shared_ptr odom) From 53ac05b5769c71296ec6d4f9aacd8eeff234f008 Mon Sep 17 00:00:00 2001 From: vvictorrr <70972941+vvictorrr@users.noreply.github.com> Date: Mon, 10 Nov 2025 15:27:41 -0500 Subject: [PATCH 6/6] Update src/purepusuit.cpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- src/purepusuit.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/purepusuit.cpp b/src/purepusuit.cpp index e7d7ad0..7d4f3b0 100644 --- a/src/purepusuit.cpp +++ b/src/purepusuit.cpp @@ -112,7 +112,8 @@ void AsyncAdaptivePurePursuitController::runLoop() { std::cout << "[PurePursuit] Starting control loop..." << std::endl; while (running) { - Pose pose = odom->getPose(); + Pose pose; + odom->getPose(&pose); PathPoint target = findLookaheadPoint(pose, lookaheadDist); double curvature = computeCurvature(pose, target);