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/purepursuit.h b/include/purepursuit.h new file mode 100644 index 0000000..e87cf5a --- /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" +#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 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/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 diff --git a/src/purepusuit.cpp b/src/purepusuit.cpp new file mode 100644 index 0000000..7d4f3b0 --- /dev/null +++ b/src/purepusuit.cpp @@ -0,0 +1,133 @@ +#include "purepursuit.h" +#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 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; // 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; +} \ No newline at end of file