PathPlannerAuto.h

#pragma once
 
#include <frc2/command/CommandHelper.h>
#include <frc2/command/button/Trigger.h>
#include <frc/geometry/Pose2d.h>
#include <wpi/json.h>
#include <string>
#include <memory>
#include <vector>
#include <frc/event/EventLoop.h>
#include <functional>
#include <frc/Timer.h>
#include "pathplanner/lib/path/PathPlannerPath.h"
#include "pathplanner/lib/events/EventTrigger.h"
#include "pathplanner/lib/events/PointTowardsZoneTrigger.h"
#include "pathplanner/lib/auto/AutoBuilder.h"
 
namespace pathplanner {
class PathPlannerAuto: public frc2::CommandHelper<frc2::Command, PathPlannerAuto> {
public:
    static std::string currentPathName;
 
    PathPlannerAuto(std::string autoName);
 
    PathPlannerAuto(std::string autoName, bool mirror);
 
    PathPlannerAuto(frc2::CommandPtr &&autoCommand, frc::Pose2d startingPose =
            frc::Pose2d());
 
    static std::vector<std::shared_ptr<PathPlannerPath>> getPathGroupFromAutoFile(
            std::string autoName);
 
    constexpr frc::Pose2d getStartingPose() const {
        return m_startingPose;
    }
 
    inline frc2::Trigger condition(std::function<bool()> condition) {
        return frc2::Trigger(m_autoLoop.get(), condition);
    }
 
    inline frc2::Trigger isRunning() {
        return condition([this]() {
            return m_isRunning;
        });
    }
 
    inline frc2::Trigger timeElapsed(units::second_t time) {
        return condition([this, time]() {
            return m_timer.HasElapsed(time);
        });
    }
 
    inline frc2::Trigger timeRange(units::second_t startTime,
            units::second_t endTime) {
        return condition([this, startTime, endTime]() {
            return m_timer.Get() >= startTime && m_timer.Get() <= endTime;
        });
    }
 
    inline EventTrigger event(std::string eventName) {
        return EventTrigger(m_autoLoop.get(), eventName);
    }
 
    inline PointTowardsZoneTrigger pointTowardsZone(std::string zoneName) {
        return PointTowardsZoneTrigger(m_autoLoop.get(), zoneName);
    }
 
    inline frc2::Trigger activePath(std::string pathName) {
        return condition([pathName]() {
            return pathName == PathPlannerAuto::currentPathName;
        });
    }
 
    inline frc2::Trigger nearFieldPosition(frc::Translation2d fieldPosition,
            units::meter_t tolerance) {
        return condition(
                [fieldPosition, tolerance]() {
                    return AutoBuilder::getCurrentPose().Translation().Distance(
                            fieldPosition) <= tolerance;
                });
    }
 
    frc2::Trigger nearFieldPositionAutoFlipped(
            frc::Translation2d blueFieldPosition, units::meter_t tolerance);
 
    frc2::Trigger inFieldArea(frc::Translation2d boundingBoxMin,
            frc::Translation2d boundingBoxMax);
 
    frc2::Trigger inFieldAreaAutoFlipped(frc::Translation2d blueBoundingBoxMin,
            frc::Translation2d blueBoundingBoxMax);
 
    void Initialize() override;
 
    void Execute() override;
 
    bool IsFinished() override;
 
    void End(bool interrupted) override;
 
private:
    std::unique_ptr<frc2::Command> m_autoCommand;
    frc::Pose2d m_startingPose;
 
    // Use a unique_ptr to avoid delted copy constructor shenanigans
    std::unique_ptr<frc::EventLoop> m_autoLoop;
    frc::Timer m_timer;
    bool m_isRunning;
 
    static std::vector<std::shared_ptr<PathPlannerPath>> pathsFromCommandJson(
            const wpi::json &json, bool choreoPaths);
 
    void initFromJson(const wpi::json &json, bool mirror);
 
    static int m_instances;
};
}